Table of contents
1. What is the motion communication model
2. Create a workspace
1. Create a function package
mkdir -p ~/catkin_ws/src
cd catkin_ws/src/
catkin_create_pkg learn_action std_msgs rospy roscpp
2. Compile the function package
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash
3. Action programming
1. Define the action file
Create an action file under the learn_action file
Create a TurtleMove.action file under the action file, and enter the code in the TurtleMove.action file:
# 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. Create a .cpp file
Under the src folder of learn_action, create a TurtleMove_server.cpp file and a TurtleMove_client.cpp file
1) TurtleMove_server.cpp file
/* 此程序通过通过动作编程实现由client发布一个目标位置 然后控制Turtle运动到目标位置的过程 */
#include <ros/ros.h> #include <actionlib/server/simple_action_server.h>
#include "learn_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
typedef actionlib::SimpleActionServer<learn_action::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& 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;
}
// 收到action的goal后调用该回调函数
void execute(const learn_action::TurtleMoveGoalConstPtr& goal, Server* as)
{
learn_action::TurtleMoveFeedback feedback;
ROS_INFO("TurtleMove is working.");
turtle_target_pose.x=goal->turtle_target_x;
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(); } // 当action完成后，向客户端返回结果 ROS_INFO("TurtleMove is finished.");
as->setSucceeded();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "TurtleMove_server");
ros::NodeHandle n,turtle_node;
ros::Subscriber sub =turtle_node.subscribe("turtle1/pose",10,&posecallback);//订阅小乌龟的位置信息
turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);//发布控制小乌龟运动的速度
// 定义一个服务器
Server server(n, "TurtleMove", boost::bind(&execute, _1, &server), false);
// 服务器开始运行
server.start();
ROS_INFO("server has started.");
ros::spin();
return 0;
}
2) TurtleMove_client.cpp file
#include <actionlib/client/simple_action_client.h>
#include "learn_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
typedef actionlib::SimpleActionClient<learn_action::TurtleMoveAction> Client;
struct Myturtle
{
float x;
float y;
float theta;
}turtle_present_pose;
// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state, const learn_action::TurtleMoveResultConstPtr& result)
{
ROS_INFO("Yay! The TurtleMove is finished!");
ros::shutdown();
}
// 当action激活后会调用该回调函数一次
void activeCb()
{
ROS_INFO("Goal just went active");
}
// 收到feedback后调用该回调函数
void feedbackCb(const learn_action::TurtleMoveFeedbackConstPtr& 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, "TurtleMove_client");
// 定义一个客户端
Client client("TurtleMove", true);
// 等待服务器端
ROS_INFO("Waiting for action server to start.");
client.waitForServer();
ROS_INFO("Action server started, sending goal.");
// 创建一个action的goal
learn_action::TurtleMoveGoal goal;
goal.turtle_target_x = 1;
goal.turtle_target_y = 1;
goal.turtle_target_theta = 0;
// 发送action的goal给服务器端，并且设置回调函数
client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
ros::spin();
return 0;
}
3) Modify the package.xml file as shown in the figure
<build_depend>message_generation</build_depend>
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
4）修改 ~/catkin_ws/src/learn_action/CMakeLists.txt
Modify as shown in the picture below:
Finally, add at the end:
the code is as follows:
add_executable(TurtleMove_client src/TurtleMove_client.cpp)
target_link_libraries(TurtleMove_client ${
catkin_LIBRARIES})
add_dependencies(TurtleMove_client ${
PROJECT_NAME}_gencpp)
add_executable(TurtleMove_server src/TurtleMove_server.cpp)
target_link_libraries(TurtleMove_server ${
catkin_LIBRARIES})
add_dependencies(TurtleMove_server ${
PROJECT_NAME}_gencpp)
3. Compile and run
The compilation process is shown in the figure:
set environment variables:
source ~/catkin_ws/devel/setup.bash
Then start running the little turtle:
roscore
source ~/catkin_ws/devel/setup.bash
rosrun turtlesim turtlesim_node
source ~/catkin_ws/devel/setup.bash
rosrun learn_action TurtleMove_server
source ~/catkin_ws/devel/setup.bash
rosrun learn_action TurtleMove_client
As best shown, the little turtle successfully runs:
4. Distributed communication
Demonstrate distributed communication of ROS on two computers
1. Host
The two computers are guaranteed to be in the same LAN, enter the command to view the host ip
ifconfig
open ros
roscore
new terminal
export ROS_IP=xxx.xxx.xxx #本机IP
export ROS_MASTER_URI=http://xxx.xxx.xxx:11311/ #主机IP
rosrun turtlesim turtlesim_node
source ~/.bashrc
2. Slave
Open a terminal, enter
export ROS_IP=X.X.X.X #本机ip
export ROS_MASTER_URI=http://x.x.x.x:11311 #主机IP
source ~/.bashrc
rosrun turtlesim turtle_teleop_key
3. Running results
I don't have two computers here, so I used one computer to open two virtual machines at the same time for testing.
As a result, it can run successfully. Use the arrow keys on one virtual machine to manipulate the little turtle running program opened on another virtual machine.
V. Summary
Motion programming is a question-and-answer communication mechanism with continuous feedback that can be aborted during a task. There are a lot of things to learn in this experiment task, and it takes a lot of time to learn and digest. It is relatively easy to complete under the guidance of the seniors' meticulous blog.
6. References
https://blog.csdn.net/qq_42585108/article/details/104852100