ROS与C++学习总结一：对例程pub与sub的C++topic理解以及自己的sub文件

最新推荐文章于 2023-07-31 10:39:03 发布

lj2jq

最新推荐文章于 2023-07-31 10:39:03 发布

阅读量2k

点赞数 1

分类专栏： ROS C++ 文章标签： ROS C++

本文链接：https://blog.csdn.net/qq_34897331/article/details/100730183

版权

ROS 同时被 2 个专栏收录

12 篇文章 0 订阅

订阅专栏

C++

3 篇文章 0 订阅

订阅专栏

ROS与C++学习总结一：对例程pub与sub的C++topic理解以及自己的sub文件

Talker源码理解
Listener源码解析
尝试写一个MarkerPose sub
- C++ MarkerPose源文件
- 主要修改部分

Talker源码理解

// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%
#include "ros/ros.h"
// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include "std_msgs/String.h"
// %EndTag(MSG_HEADER)%
#include <sstream>

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line.
   * For programmatic remappings you can use a different version of init() which takes
   * remappings directly, but for most command-line programs, passing argc and argv is
   * the easiest way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
// %Tag(INIT)%
  ros::init(argc, argv, "talker");
// %EndTag(INIT)%

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
// %Tag(NODEHANDLE)%
  ros::NodeHandle n;
// %EndTag(NODEHANDLE)%

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
// %Tag(PUBLISHER)%
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
// %EndTag(PUBLISHER)%

// %Tag(LOOP_RATE)%
  ros::Rate loop_rate(10);
// %EndTag(LOOP_RATE)%

  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */
// %Tag(ROS_OK)%
  int count = 0;
  while (ros::ok())
  {
// %EndTag(ROS_OK)%
    /**
     * This is a message object. You stuff it with data, and then publish it.
     */
// %Tag(FILL_MESSAGE)%
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();
// %EndTag(FILL_MESSAGE)%

// %Tag(ROSCONSOLE)%
    ROS_INFO("%s", msg.data.c_str());
// %EndTag(ROSCONSOLE)%

    /**
     * The publish() function is how you send messages. The parameter
     * is the message object. The type of this object must agree with the type
     * given as a template parameter to the advertise<>() call, as was done
     * in the constructor above.
     */
// %Tag(PUBLISH)%
    chatter_pub.publish(msg);
// %EndTag(PUBLISH)%

// %Tag(SPINONCE)%
    ros::spinOnce();
// %EndTag(SPINONCE)%

// %Tag(RATE_SLEEP)%
    loop_rate.sleep();
// %EndTag(RATE_SLEEP)%
    ++count;
  }


  return 0;
}
// %EndTag(FULLTEXT)%

ros::init(argc, argv, "talker");

初始化ROS，指定节点名称为“talker”，节点名称要保持唯一性。

ros::NodeHandle n;

实例化节点
问题：n与talker是什么关系？
理解：talker仅为节点名称，n为节点句柄用来处理后续给topic传递消息

  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

定义了一个发布者，名字为chatter，数据类型为std_msgs::String，数值1000定义消息队列大小为1000，即超过1000条消息之后，旧的消息就会丢弃。

    std_msgs::String msg;

定义了message object要与主题中的数据类型对应。

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

定义了字符串流，可以把其他数据类型转换到string，例如本代码中的int count通过<<赋值与“hello world ”一同转化为string存入了ss中了
由于msg的变量为data，因而通过msg.data 利用ss.str（）将ss中的值存入

    ROS_INFO("%s", msg.data.c_str());

类似于C++中的std::cout用来输出打印信息，“%s”表明数据类型为String，由于msg.data的数据类型为string，因而可以通过c_str（）来调用其中的值

    chatter_pub.publish(msg);

发布消息，括号内为message object

    ros::spinOnce();

回调一次，此处无任何卵用，不是必需的，但是保持增加这个调用，是好习惯。
如果程序里也有订阅话题，就必需，否则回调函数不能起作用。

Listener源码解析

#include "ros/ros.h"
#include "std_msgs/String.h"

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line.
   * For programmatic remappings you can use a different version of init() which takes
   * remappings directly, but for most command-line programs, passing argc and argv is
   * the easiest way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "listener");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
  ros::spin();

  return 0;
}

订阅代码相对来说比较简单主要是写回调函数注意变量名称，主题名。

chatterCallback（）
copy from https://www.jianshu.com/p/06e0e40cf6da.
特别感谢上面的作者，写了了1, 2, 3部分关于ROS下Pub与Sub的c++源文件编写，尤其是第三部分特别有用，直接促成了下面的MarkerPose c++源文件的编写成功。下文引用了上面链接中讲解的最重要的部分。

我们看回调函数。不熟悉C++的同学可能要会想为什么用这个函数的参数长这个样子。 (const std_msgs::String::ConstPtr& msg)什么鬼? 首先我得说，这个模式还是比较固定的，如果你要接受的是Int8类型的消息，那么参数会变成(const std_msgs::Int8::ConstPtr& msg)。ConstPtr代表一个指针。所以你目前要知道的是msg这个参数是你接收到的消息的指针就行了，同样这个名字你也随意改，但一般是…msg。所以你看到printf(ROS_INFO)中有msg->data这种调用方式。（在Publisher中我们定义了std_msgs::String对象msg，类包含数据成员data，调用方式为msg.data。如果类的指针叫msg,那么调用该成员的方式是msg->data）。所以现在msg->data就是一个std::string类型的量，假设有string类型的变量st要想print出来,代码就是printf("%s,", st.c_str() )（不能直接print st）。使用ROS_INFO其内部完全一样。

尝试写一个MarkerPose sub

ljq@li2jq:~$ rostopic type /aruco_single/pose
geometry_msgs/PoseStamped
ljq@li2jq:~$ rosmsg show geometry_msgs/PoseStamped
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
对应topic的数据类型

ljq@li2jq:~$ rostopic type /chatter
std_msgs/String
ljq@li2jq:~$ rosmsg show st
std_msgs/ stereo_msgs/
ljq@li2jq:~$ rosmsg show st
std_msgs/ stereo_msgs/
ljq@li2jq:~$ rosmsg show std_msgs/String
string data

仿照参考

C++ MarkerPose源文件

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
//参考sub文件写一个ArucoMarker Pose读取的subscribe
void MarkerCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{

    ROS_INFO("I heard the pose from the robot"); 
    ROS_INFO("the position(x,y,z) is %f , %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    ROS_INFO("the orientation(x,y,z,w) is %f , %f, %f, %f", msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
    ROS_INFO("the time we get the pose is %f",  msg->header.stamp.sec + 1e-9*msg->header.stamp.nsec);

    std::cout<<"\n \n"<<std::endl; //add two more blank row so that we can see the message more clearly
}

int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line.
   * For programmatic remappings you can use a different version of init() which takes
   * remappings directly, but for most command-line programs, passing argc and argv is
   * the easiest way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "Marker");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  ros::Subscriber Marke_sub = n.subscribe("aruco_single/pose", 1000, MarkerCallback);

  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */
  ros::spin();

  return 0;
}

主体代码copy from listener.cpp文件，该代码实现了订阅Aruco_single/pose topic来查看Pose的信息，先通过两个launch命令启动kinect2摄像头与二维码识别：
roslaunch aruco_ros single.launch
roslaunch kinect2_bridge kinect2_bridge.launch

主要修改部分

#include “geometry_msgs/PoseStamped.h” 该文件是必须要添加的，应为Aruco_Single/Pose这个topic的type为geometry_msgs/PoseStamped，数据类型在上面有提到
callback函数编写：

void MarkerCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)

主要是参数部分的格式，const后面要与topic的type对应。

由于调入的是个指针msg，因而后面调用都用msg->的形式
主函数中的sub函数：

  ros::Subscriber Marke_sub = n.subscribe("aruco_single/pose", 1000, MarkerCallback);

这个函数注意第一个参数为订阅的topic，前面没有（/）。

对CMakeLists.txt的修改：
（1）find_package 中要加入相应的包
（2)
add_executable(MarkerPose src/MarkerPose.cpp)
target_link_libraries(MarkerPose ${catkin_LIBRARIES})
add_dependencies(MarkerPose beginner_tutorials_generate_messages_cpp)
加入对应的执行，库，以及依赖文件
对package.xml的修改：
<build_depend>geometry_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
加入对应depend文件。