Tip: After the article is written, the table of contents can be automatically generated. How to generate it can refer to the help document on the right
Article directory
- Programming implementation process
- Summarize
- reference link
Programming implementation process
1. Open the terminal and enter the project folder
2) Create a new service file turtleMove.cpp
Command: touch turtleMove.cpp
3) Write the code:
Edit the touch turtleMove.cpp file
Command: gedit turtleMove.cpp
Enter the following code
/*
This program implements a target location published by the client through action programming
Then control the process of Turtle moving to the target position
*/
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
typedef actionlib::SimpleActionServer<comm::turtleMoveAction> Server;
struct Myturtle
{<!-- -->
float x;
float y;
float theta;
}turtle_original_pose,turtle_target_pose;
ros::Publisher turtle_vel;
void posecallback(const turtlesim::PoseConstPtr & amp; msg)
{<!-- -->
ROS_INFO("turtle1_position:(%f,%f,%f)",msg->x,msg->y,msg->theta);
turtle_original_pose.x=msg->x;
turtle_original_pose.y=msg->y;
turtle_original_pose.theta=msg->theta;
}
// Call the callback function after receiving the goal of the action
void execute(const comm::turtleMoveGoalConstPtr & amp; goal, Server* as)
{<!-- -->
comm::turtleMoveFeedback feedback;
ROS_INFO("TurtleMove is working.");
turtle_target_pose.x=goal->turtle_target_x;
- 3 -
turtle_target_pose.y=goal->turtle_target_y;
turtle_target_pose.theta=goal->turtle_target_theta;
geometry_msgs::Twist vel_msgs;
float break_flag;
while(1)
{<!-- -->
ros::Rate r(10);
vel_msgs.angular.z = 4.0*
(atan2(turtle_target_pose.y-turtle_original_pose.y,
turtle_target_pose.x-turtle_original_pose.x)-turtle_original_pose.theta);
vel_msgs.linear.x = 0.5*
sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
pow(turtle_target_pose.y-turtle_original_pose.y, 2));
break_flag=sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
pow(turtle_target_pose.y-turtle_original_pose.y, 2));
turtle_vel. publish(vel_msgs);
feedback.present_turtle_x=turtle_original_pose.x;
feedback.present_turtle_y=turtle_original_pose.y;
feedback.present_turtle_theta=turtle_original_pose.theta;
as->publishFeedback(feedback);
ROS_INFO("break_flag=%f", break_flag);
if(break_flag<0.1) break;
r. sleep();
}
// When the action is completed, return the result to the client
ROS_INFO("TurtleMove is finished.");
as->setSucceeded();
}
int main(int argc, char** argv)
{<!-- -->
ros::init(argc, argv, "turtleMove");
ros::NodeHandle n, turtle_node;
ros::Subscriber sub =
turtle_node.subscribe("turtle1/pose", 10, & amp;posecallback); //subscribe the position of the little turtle
information
turtle_vel =
turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);//Release control
Control the speed of the little turtle's movement
// define a server
- 4 -
Server server(n, "turtleMove", boost::bind( & amp;execute, _1,
& amp; server), false);
// server starts running
server.start();
ROS_INFO("server has started.");
ros::spin();
return 0;
}
2. Create the little turtle “publish target location file” turtleMoveClient.cpp
In the same directory, create the “Publish Target Location” file turtleMoveClient.cpp
Command: touch turtleMoveClient.cpp
2) Write the code:
Command: gedit turtleMoveClient.cpp
Enter the following code
#include <actionlib/client/simple_action_client.h>
#include "my_turtle_package/turtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
typedef actionlib::SimpleActionClient<my_turtle_package::turtleMoveAction>
Client;
struct Myturtle
{<!-- -->
float x;
float y;
float theta;
}turtle_present_pose;
// The callback function will be called once when the action is completed
void doneCb(const actionlib::SimpleClientGoalState & amp;state,
const my_turtle_package::turtleMoveResultConstPtr & amp;result)
{<!-- -->
ROS_INFO("Yay! The turtleMove is finished!");
ros::shutdown();
}
// This callback function will be called once when the action is activated
void activeCb()
{<!-- -->
ROS_INFO("Goal just went active");
}
// Call this callback function after receiving feedback
void feedbackCb(const my_turtle_package::turtleMoveFeedbackConstPtr & amp;feedback)
{<!-- -->
ROS_INFO(" present_pose : %f %f %f",
feedback->present_turtle_x,
feedback->present_turtle_y, feedback->present_turtle_theta);
}
int main(int argc, char** argv)
{<!-- -->
ros::init(argc, argv, "turtleMoveClient");
// define a client
Client client("turtleMove", true);
// wait for server side
ROS_INFO("Waiting for action server to start.");
client.waitForServer();
ROS_INFO("Action server started, sending goal.");
// create an action goal
my_turtle_package::turtleMoveGoal goal;
goal.turtle_target_x = 1;
goal.turtle_target_y = 1;
goal.turtle_target_theta = 0;
// Send the goal of the action to the server and set the callback function
client.sendGoal(goal, & amp;doneCb, & amp;activeCb, & amp;feedbackCb);
ros::spin();
return 0;
}
3. Create an action folder in the function package directory
Enter the project package directory
Command: cd ~/catkin_ws/src/my_turtle_package
Create action folder
Command: mkdir action
4. Create the turtleMove.action file under the action folder
, enter the action folder
Command: cd action
, create the turtleMove.action file
Command: touch turtleMove.action
, Write the following code into the turtleMove.action file, as follows:
Command: gedit turtleMove.action
# Define the goal float64 turtle_target_x # Specify Turtle's target position float64 turtle_target_y float64 turtle_target_theta --- # Define the result float64 turtle_final_x float64 turtle_final_y float64 turtle_final_theta --- # Define a feedback message float64 present_turtle_x float64 present_turtle_y float64 present_turtle_theta
2. Modify the content of the CMakeLists.txt file
1 Enter the project package my_turtle_package directory
Command: cd ~/catkin_ws/src/my_turtle_package
2 Open the CMakeLists.txt file
Command: sudo gedit CMakeLists.txt
3 Add the following code to the end of the CMakeLists.txt file
add_executable(turtleMoveClient src/turtleMoveClient.cpp)
target_link_libraries(turtleMoveClient ${<!-- -->catkin_LIBRARIES})
add_dependencies(turtleMoveClient ${<!-- -->PROJECT_NAME}_gencpp)
add_executable(turtleMove src/turtleMove.cpp)
target_link_libraries(turtleMove ${<!-- -->catkin_LIBRARIES})
add_dependencies(turtleMove ${<!-- -->PROJECT_NAME}_gencpp)
2) Find the find_package function method in this file and modify it as follows:
3) Continue to find the add_action_files function in the file and modify it to the following code:
add_action_files( FILES turtleMove. action )
4) Continue to find the generate_messages function item in the file
5) Find the catkin_package function and modify it to the following code:
3. Modify the content of the package.xml file
1 Open the package.xml file
Command: gedit package.xml
2 Replace the build_depend column in the file with the following code:
<buildtool_depend>catkin</buildtool_depend> <build_depend>roscpp</build_depend> <build_depend>rospy</build_depend> <build_depend>std_msgs</build_depend> <build_depend>message_generation</build_depend> <build_depend>actionlib</build_depend> <build_depend>actionlib_msgs</build_depend> <build_export_depend>roscpp</build_export_depend> <build_export_depend>rospy</build_export_depend> <build_export_depend>std_msgs</build_export_depend>
3 Replace the exec_depend column in the file with the following code:
<exec_depend>roscpp</exec_depend> <exec_depend>rospy</exec_depend> <exec_depend>std_msgs</exec_depend> <exec_depend>message_runtime</exec_depend> <exec_depend>actionlib</exec_depend> <exec_depend>actionlib_msgs</exec_depend>
and then compile
catkin_make
After successful compilation is as follows
Then we can open ros to start the little turtle to move the little turtle to the specified position
Summary
This experiment mainly allows us to further learn ros programming through the example of the little turtle, realize communication, monitor the movement of the little turtle in real time, and get position feedback.
Reference link
https://blog.csdn.net/mxh3600/article/details/125243263
https://blog.csdn.net/qq_44989881/article/details/118635001