ROS基础

话题编程

实现发布者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坐标变换编程

先建一个功能包

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>