话题编程
实现发布者talker.cpp
1 #include <sstream> 2 #include "ros/ros.h" 3 #include "std_msgs/String.h" 4 5 int main(int argc, char **argv) 6 { 7 //ROS节点初始化 8 ros::init(argc, argv, "talker"); 9 10 //创建节点句柄 11 ros::NodeHandle n; 12 13 //创建一个publisher,发布名为chatter的topic,消息类型为std_msgs::String 14 ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000); 15 16 //设置循环的频率 17 ros::Rate loop_rate(10); 18 19 int count = 0; 20 while (ros::ok()) 21 { 22 //初始化std_msgs::String类型的消息 23 std_msgs::String msg; 24 std::stringstream ss; 25 ss << "hello world" << count; 26 msg.data = ss.str(); 27 28 //发布消息 29 ROS_INFO("%s", msg.data.c_str()); 30 chatter_pub.publish(msg);//发布 31 32 //循环等待回调函数 33 ros::spinOnce(); 34 35 //按照循环频率延时 36 loop_rate.sleep(); 37 ++count; 38 } 39 return 0; 40 }
实现订阅者listener.cpp
1 #include "ros/ros.h" 2 #include "std_msgs/String.h" 3 4 //接收到订阅的消息后，会进入消息回调函数 5 void chatterCallback(const std_msgs::String::ConstPtr& msg) 6 { 7 //将接收到的消息打印出来 8 ROS_INFO("I heard: [%s]", msg->data.c_str()); 9 } 10 11 int main(int argc, char **argv) 12 { 13 //初始化ROS节点 14 ros::init(argc, argv, "listener"); 15 16 //创建节点句柄 17 ros::NodeHandle n; 18 19 //创建一个Subscriber, 订阅名为chatter的topic,注册回调函数chatterCallback 20 ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); 21 22 //循环等待回调函数 23 ros::spin(); 24 25 return 0; 26 }
设置需要编译的代码和生成的可执行文件；设置链接库；设置依赖。CMakeLists.txt
1 cmake_minimum_required(VERSION 2.8.3) 2 project(learning_communication) 3 4 find_package(catkin REQUIRED COMPONENTS 5 roscpp 6 rospy 7 std_msgs 8 ) 9 10 catkin_package( 11 # INCLUDE_DIRS include 12 # LIBRARIES learning_communication 13 # CATKIN_DEPENDS roscpp rospy std_msgs 14 # DEPENDS system_lib 15 ) 16 17 include_directories( 18 # include 19 ${catkin_INCLUDE_DIRS} 20 ) 21 22 add_executable(talker src/talker.cpp) 23 24 add_executable(listener src/listener.cpp) 25 26 target_link_libraries(talker 27 ${catkin_LIBRARIES} 28 ) 29 30 target_link_libraries(listener 31 ${catkin_LIBRARIES} 32 )
listener.cpp,talker.cpp,CMakeLists.txt三部分。
发布
订阅
１．在msg/Person.msg中定义msg文件
string name uint8 sex uint8 age uint8 unknown = 0 uint8 male = 1 uint8 female = 2
２．在package.xml中添加功能包依赖
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
将package.xml中的注释去掉即可。
３．在CMakeLists.txt添加编译选项（与消息有关）
find_package(...message_generation)
catkin_package(CATKIN_DEPENDS roscpp rospy std_msgs message_runtime)
add_message_files( FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)
编译后，查看自定义消息是否成功
服务编程
１.在srv/AddTwoInts.srv中自定义服务请求与应答
int64 a int64 b --- int64 sum
２．在package.xml中添加功能包依赖(同上)
３．在CMakeLists.txt添加编译选项（与服务有关）
add_service_files( FILES AddTwoInts.srv) 部分内容已在消息编程中添加。
实现服务器server.cpp
1 #include "ros/ros.h" 2 #include ""learning_communication/AddTwoInts.h" 3 4 //service回调函数，输入参数req,输出参数res 5 bool add(learning_communication::AddTwoInts::Request &req, 6 learning_communication::AddTwoInts::Response &res) 7 { 8 //将输入参数中的请求数据相加，结果放到应答变量中 9 res.sum = req.a + req.b; //req 10 ROS_INFO("request: x=%d, y=%d", (long int)req.a, (long int)req.b); 11 ROS_INFO("sending back response: [%d]", (long int)res.sum);//res 12 13 return true; 14 } 15 16 int main(int argc, char **argv) 17 { 18 // ROS节点初始化 19 ros::init(argc, argv, "add_two_ints_server"); 20 21 //创建节点句柄 22 ros::NodeHandle n; 23 24 //创建一个名为add_two_ints的server,注册回调函数add() 25 ros::ServiceServer service = n.advertiseSever("add_two_ints", add);//add_two_ints服务名 26 27 //循环等待回调函数 28 ROS_INFO("Ready to add ints."); 29 ros::spin(); 30 31 return 0; 32 }
实现客户端client.cpp
1 #include <cstlib> 2 #include "ros/ros.h" 3 #include "learning_communication/AddTwoInts.h" 4 5 int main(int argc, char **argv) 6 { 7 //ROS节点初始化 8 ros::init(argc, argv, "add_two_ints_client"); 9 10 //从终端命令行获取两个加数 11 if (argc != 3) 12 { 13 ROS_INFO("usage: add_two_ints_client X Y"); 14 return 1; 15 } 16 17 //创建节点句柄 18 ros::NodeHandle n; 19 20 //创建一个client,请求add_two_int service 21 //service消息类型是learning_communication::AddTwoInts 22 ros::ServiceClient client = n.serviceClient<learning_communication::AddTwoInts>("add_two_ints"); 23 24 //创建learning_communication::AddTwoInts类型的service消息 25 learning_communication::AddTwoInts srv; 26 srv.request.a = atoll(argv[1]); 27 srv.request.b = atoll(argv[2]); 28 29 //发布service请求，等待加法运算的应答结果 30 if (client.call(srv)) 31 { 32 ROS_INFO("Sum: %d", (long int)srv.response.sum); 33 } 34 else 35 { 36 ROS_INFO("Failed to call service add_two_ints"); 37 return 1; 38 } 39 40 return 0; 41 }
启动Server节点
等待
客户端发布服务请求
服务端接收到请求，完成加法，将结果发给客户端。
动作编程
１.在action/DoDishes.action中自定义动作消息
#定义目标信息 uint32 dishwasher_id #Specify which dishwasher we want to use --- #定义结果信息 uint32 total_dishes_cleaned --- #定义周期反馈的消息 float32 percent_complete
２．在package.xml中添加功能包依赖
<build_depend>actionlib</build_depend> <build_depend>actionlib_msgs</build_depend> <exec_depend>actionlib</exec_depend> <exec_depend>actionlib_msgs</exec_depend>
３．在CMakeLists.txt添加编译选项
find_package(catkin REQUIRED actionlib_msgs actionlib)
add_action_files(DIRECTORY action FILES DoDishes.action)
generate_messages(DEPENDENCIES actionlib_msgs)
在原编译选项中添加，不应完全复制进CMakeLists.txt中。
实现动作服务器DoDishes_server.cpp
1 #include <ros/ros.h> 2 #include <actionlib/server/simple_action_server.h> 3 #include "learning_communication/DoDishesAction.h" 4 5 typedef actionlib::SimpleActionServer<learning_communication::DoDishesAction> Server; 6 7 //收到action的goal后调用该回归函数 8 void execute(const learning_communication::DoDishesGoalConstPtr& goal, Server* as) 9 { 10 ros::Rate r(1); 11 learning_communication::DoDishesFeedback feedback; 12 13 ROS_INFO("Disheswasher %d is working.", goal->dishwasher_id); 14 15 //假设洗盘子的进度，并且按照１ＨＺ的频率发布进度feedback 16 for (int i = 1; i <= 10; i++) 17 { 18 feedback.percent_complete = i * 10; 19 as->publishFeedback(feedback); 20 r.sleep(); 21 } 22 23 //当action完成后，向客户端返回结果 24 ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id); 25 as->setSucceeded(); 26 } 27 28 int main(int argc, char** argv) 29 { 30 ros::init(argc, argv, "do_dishes_server"); 31 ros::NodeHandle n; 32 33 //定义一个服务器 34 Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false); 35 36 //服务器开始运行 37 server.start(); 38 39 ros::spin(); 40 return 0; 41 }
实现动作客户端DoDishes_client.cpp
1 #include <actionlib/client/simple_action_client.h> 2 #include "learning_communication/DoDishesAction.h" 3 4 typedef actionlib::SimpleActionClient<learning_communication::DoDishesAction> Client; 5 6 //当action完成后会调用该回调函数一次 7 void doneCb(const actionlib::SimpleClientGoalState& state, const learning_communication::DoDishesResultConstPtr& result) 8 { 9 ROS_INFO("Yay! The dishes are now clean"); 10 ros::shutdown(); 11 } 12 13 //当action激活后会调用该回调函数一次 14 void activeCb() 15 { 16 ROS_INFO("Goal just went active"); 17 } 18 19 //收到feedback后调用该回调函数 20 void feedbackCb(const learning_communication::DoDishesFeedbackConstPtr& feedback) 21 { 22 ROS_INFO(" percent_complete : %f ", feedback->percent_complete); 23 } 24 25 int main(int argc, char** argv) 26 { 27 28 ros::init(argc, argv, "do_dishes_client"); 29 30 //定义一个客户端 31 Client client("do_dishes", true); 32 33 //等待服务器端 34 ROS_INFO("Waiting for action server to start."); 35 client.waitForServer(); 36 ROS_INFO("Action server started, sending goal."); 37 38 //创建一个action的goal 39 learning_communication::DoDishesGoal goal; 40 goal.dishwasher_id = 1; 41 42 //发送action的goal给服务器端，并且设置回调函数 43 client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); 44 45 ros::spin(); 46 47 return 0; 48 }
运行过程中
运行结束
TF坐标变换
roslaunch turtle_tf turtle_tf_demo.launch
往另一个乌龟运动。
rosrun turtlesim turtle_teleop_key
控制一只乌龟运动，另一只乌龟追。
rosrun tf view_frames
监听５秒，生成ＰＤＦ文件。
实时监听输出坐标变换信息
（可以看出只有平移变换）
TF坐标变换编程
先建一个功能包
应该在catkin_learning/src/下输入命令，上图有错误。
TF广播器turtle_tf_broadcaster.cpp
1 #include <ros/ros.h> 2 #include <tf/transform_broadcaster.h> 3 #include <turtlesim/Pose.h> 4 5 std::string turtle_name; 6 7 void poseCallback(const turtlesim::PoseConstPtr& msg) 8 { 9 // tf广播器 10 static tf::TransformBroadcaster br; 11 12 // 根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换 13 tf::Transform transform; 14 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); 15 tf::Quaternion q; 16 q.setRPY(0, 0, msg->theta);//z轴转 17 transform.setRotation(q); 18 19 // 发布坐标变换 20 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); 21 } 22 23 int main(int argc, char** argv) 24 { 25 // 初始化节点 26 ros::init(argc, argv, "my_tf_broadcaster"); 27 if (argc != 2) 28 { 29 ROS_ERROR("need turtle name as argument"); 30 return -1; 31 }; 32 turtle_name = argv[1]; 33 34 // 订阅乌龟的pose信息 35 ros::NodeHandle node; 36 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); 37 38 ros::spin(); 39 40 return 0; 41 };
1 #include <ros/ros.h> 2 #include <tf/transform_listener.h> 3 #include <geometry_msgs/Twist.h> 4 #include <turtlesim/Spawn.h> 5 6 int main(int argc, char** argv) 7 { 8 // 初始化节点 9 ros::init(argc, argv, "my_tf_listener"); 10 11 ros::NodeHandle node; 12 13 // 通过服务调用，产生第二只乌龟turtle2 14 ros::service::waitForService("spawn"); 15 ros::ServiceClient add_turtle = 16 node.serviceClient<turtlesim::Spawn>("spawn"); 17 turtlesim::Spawn srv; 18 add_turtle.call(srv); 19 20 // 定义turtle2的速度控制发布器 21 ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10); 22 23 // tf监听器 24 tf::TransformListener listener; 25 26 ros::Rate rate(10.0); 27 while (node.ok()) 28 { 29 tf::StampedTransform transform; 30 try 31 { 32 // 查找turtle2与turtle1的坐标变换 33 listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));//等待时间 34 listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);//变换关系放在transform 35 } 36 catch (tf::TransformException &ex) 37 { 38 ROS_ERROR("%s",ex.what()); 39 ros::Duration(1.0).sleep(); 40 continue; 41 } 42 43 // 根据turtle1和turtle2之间的坐标变换，计算turtle2需要运动的线速度和角速度 44 // 并发布速度控制指令，使turtle2向turtle1移动 45 geometry_msgs::Twist vel_msg; 46 vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), 47 transform.getOrigin().x()); 48 vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + 49 pow(transform.getOrigin().y(), 2)); 50 turtle_vel.publish(vel_msg); 51 52 rate.sleep(); 53 } 54 return 0; 55 };
修改CMakeLists.txt
除此之外，在find_package中添加tf,turtlesim,std_msgs。
添加start_demo_with_listener.launch
1 <launch> 2 <!-- 海龟仿真器 --> 3 <node pkg="turtlesim" type="turtlesim_node" name="sim"/> 4 5 <!-- 键盘控制 --> 6 <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> 7 8 <!-- 两只海龟的tf广播,发布坐标关系 --> 9 <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" /> 10 <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /> 11 12 <!-- 监听tf广播，并且控制turtle2移动 --> 13 <node pkg="learning_tf" type="turtle_tf_listener" name="listener" /> 14 15 </launch>
