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: