话题编程
实现发布者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>
来源：oschina
链接：https://my.oschina.net/u/4386697/blog/3881850