ROS实践（6）-自建示例

一 单topic

1 初始化环境

创建路径~/dev/mytest，并加入环境变量：

root@yangkai04-Inspiron-3650:~/dev# pwd
/root/dev

vim ~/.bashrc
export ROS_PACKAGE_PATH=~/dev/mytest:~/dev/rosbook:/opt/ros/indigo/share:/opt/ros/indigo/stacks

2 建功能包

在路径~/dev/mytest下创建包：

root@yangkai04-Inspiron-3650:~/dev/mytest# roscreate-pkg send_and_recv std_msgs roscpp
Created package directory /root/dev/mytest/send_and_recv
Created include directory /root/dev/mytest/send_and_recv/include/send_and_recv
Created cpp source directory /root/dev/mytest/send_and_recv/src
Created package file /root/dev/mytest/send_and_recv/Makefile
Created package file /root/dev/mytest/send_and_recv/manifest.xml
Created package file /root/dev/mytest/send_and_recv/CMakeLists.txt
Created package file /root/dev/mytest/send_and_recv/mainpage.dox

Please edit send_and_recv/manifest.xml and mainpage.dox to finish creating your package
root@yangkai04-Inspiron-3650:~/dev/mytest# rospack find send_and_recv
/root/dev/mytest/send_and_recv

root@yangkai04-Inspiron-3650:~# roscd send_and_recv
root@yangkai04-Inspiron-3650:~/dev/mytest/send_and_recv# ls
CMakeLists.txt  include  mainpage.dox  Makefile  manifest.xml  src

3 创建节点

创建src/send.cpp文件：

#include "ros/ros.h"#include "std_msgs/String.h"#include <sstream>int main(int argc, char **argv) {  ros::init(argc, argv, "send");  ros::NodeHandle n;  ros::Publisher pub = n.advertise<std_msgs::String>("mine_topic1", 1000);  ros::Rate loop_rate(10);  while (ros::ok()) {    std_msgs::String msg;    std::stringstream ss;    ss << " I am the example1_a node ";    msg.data = ss.str();    //ROS_INFO("%s", msg.data.c_str());    pub.publish(msg);    ros::spinOnce();    loop_rate.sleep();  }  return 0;}

创建src/recv.cpp文件:

#include "std_msgs/String.h"#include "ros/ros.h"void messageCallback(const std_msgs::String::ConstPtr& msg) {    ROS_INFO("I heard: [%s]", msg->data.c_str());  }int main(int argc, char **argv) {  ros::init(argc, argv, "recv");    ros::NodeHandle n;    ros::Subscriber sub = n.subscribe("mine_topic1", 1000, messageCallback);    ros::spin();    return 0;  }

修改CMakeLists.txt文件：

rosbuild_add_executable(send src/send.cpp)
rosbuild_add_executable(recv src/recv.cpp)

4 编译

root@yangkai04-Inspiron-3650:~/dev/mytest/send_and_recv# rosmake send_and_recv

[ rosmake ] Results:                                                            
[ rosmake ] Built 23 packages with 0 failures.                                  
[ rosmake ] Summary output to directory                                         
[ rosmake ] /root/.ros/rosmake/rosmake_output-20161207-193309

5 启动

启动master： roscore

启动测试发送节点：rosrun send_and_recv send

启动测试接收节点：rosrun send_and_recv recv

二 多topic

修改src/send.cpp文件:

#include "ros/ros.h"#include "std_msgs/String.h"#include <sstream>int main(int argc, char **argv) {  ros::init(argc, argv, "send");  ros::NodeHandle n;  ros::Publisher pub1 = n.advertise<std_msgs::String>("mine_topic1", 1000);  ros::Publisher pub2 = n.advertise<std_msgs::String>("mine_topic2", 1000);  ros::Rate loop_rate(10);  while (ros::ok()) {    std_msgs::String msg1;    std::stringstream ss1;    ss1 << " I am the node 1";    msg1.data = ss1.str();    pub1.publish(msg1);    //ROS_INFO("%s", msg1.data.c_str());    std_msgs::String msg2;    std::stringstream ss2;    ss2 << " I am the node 2";    msg2.data = ss2.str();    pub2.publish(msg2);    //ROS_INFO("%s", msg2.data.c_str());    ros::spinOnce();    loop_rate.sleep();  }  return 0;}

修改src/recv.cpp文件:

#include "std_msgs/String.h"#include "ros/ros.h"void messageCallback(const std_msgs::String::ConstPtr& msg) {    ROS_INFO("I heard: [%s]", msg->data.c_str());  }int main(int argc, char **argv) {  ros::init(argc, argv, "recv");    ros::NodeHandle n;    ros::Subscriber sub1 = n.subscribe("mine_topic1", 1000, messageCallback);    ros::Subscriber sub2 = n.subscribe("mine_topic2", 1000, messageCallback);    ros::spin();    return 0;  }

三 异步多线程

修改src/recv.cpp文件:

#include "std_msgs/String.h"#include "ros/ros.h"void messageCallback(const std_msgs::String::ConstPtr& msg) {  ROS_INFO("I heard: [%s]", msg->data.c_str());}int main(int argc, char **argv) {  ros::init(argc, argv, "recv");  ros::NodeHandle n;  ros::AsyncSpinner spinner(4);  spinner.start();  ros::Subscriber sub1 = n.subscribe("mine_topic1", 1000, messageCallback);  ros::Subscriber sub2 = n.subscribe("mine_topic2", 1000, messageCallback);  ros::waitForShutdown();  //ros::spin();    return 0;}

四 rqt_console

原来叫rxconsole，现在叫rqt_console。启动所有节点，roscore，rosrun pkgname  nodename，然后命令行输入rqt_console：

五 rqt_graph

rxgraph改名rqt_graph

六 launch文件

root@yangkai04-Inspiron-3650:~/dev/mytest/send_and_recv# mkdir launch -p
root@yangkai04-Inspiron-3650:~/dev/mytest/send_and_recv# vim launch/send_and_recv.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <!-- Logger config -->
  <!-- <env name="ROSCONSOLE_CONFIG_FILE"  
       value="$(find chapter3_tutorials)/config/chapter3_tutorials.config"/> -->
  <!-- Example 1 -->
  <node pkg="send_and_recv" type="send" name="send"  
        output="screen"/>
  <node pkg="send_and_recv" type="recv" name="recv"  
        output="screen"/>
</launch>

root@yangkai04-Inspiron-3650:~/dev/mytest/send_and_recv# roslaunch send_and_recv send_and_recv.launch

CTRL+C

七 roswtf

root@yangkai04-Inspiron-3650:~/dev/mytest/send_and_recv# roswtf
Loaded plugin tf.tfwtf
Package: send_and_recv
================================================================================
Static checks summary:

No errors or warnings
================================================================================
Beginning tests of your ROS graph. These may take awhile...
analyzing graph...
... done analyzing graph
running graph rules...
... done running graph rules

Online checks summary:

Found 1 warning(s).
Warnings are things that may be just fine, but are sometimes at fault

WARNING The following node subscriptions are unconnected:
 * /rosout:
   * /rosout

八 rqt_plot

rxplot改名为rqt_plot，可以针对topic中的标量或矢量数据进行绘图，我们修改上面程序中topic1数据类型为Int32

src/send.cpp

#include "ros/ros.h"#include "std_msgs/String.h"#include "std_msgs/Int32.h"#include <sstream>int main(int argc, char **argv) {  ros::init(argc, argv, "send");  ros::NodeHandle n;  ros::Publisher pub1 = n.advertise<std_msgs::Int32>("mine_topic1", 1000);  ros::Publisher pub2 = n.advertise<std_msgs::String>("mine_topic2", 1000);  ros::Rate loop_rate(10);  std_msgs::Int32 num;  num.data = 0;  while (ros::ok()) {    std_msgs::Int32 msg1 = num;    num.data++;    pub1.publish(msg1);    //ROS_INFO("%s", msg1.data.c_str());    std_msgs::String msg2;    std::stringstream ss2;    ss2 << " I am the node 2";    msg2.data = ss2.str();    pub2.publish(msg2);    //ROS_INFO("%s", msg2.data.c_str());    ros::spinOnce();    loop_rate.sleep();  }  return 0;}

src/recv.cpp

#include "std_msgs/String.h"#include "std_msgs/Int32.h"#include "ros/ros.h"void messageCallback1(const std_msgs::Int32::ConstPtr& msg) {    ROS_INFO("I heard: [%d]", msg->data);  }void messageCallback2(const std_msgs::String::ConstPtr& msg) {    ROS_INFO("I heard: [%s]", msg->data.c_str());  }int main(int argc, char **argv) {  ros::init(argc, argv, "recv");    ros::NodeHandle n;    ros::AsyncSpinner spinner(4);  spinner.start();  ros::Subscriber sub1 = n.subscribe("mine_topic1", 1000, messageCallback1);    ros::Subscriber sub2 = n.subscribe("mine_topic2", 1000, messageCallback2);    ros::waitForShutdown();  //ros::spin();    return 0;  }

运行rqt_plot，选择mine_topic1: