ROS与C++入门教程-构建工作空间
构建Catkin包
搭建开发环境(catkin_make)
编写简单发布节点和订阅节点
编写简单服务端和客户端
使用类方法作为回调函数
使用Timers类
编写高级的发布器和订阅器
Callbacks和Spinning

ROS与C++入门教程-构建工作空间

创建一个catkin 工作空间：

$ mkdir -p ~/catkin_test/src
$ cd ~/catkin_test/src

即使这个工作空间是空的（在’src’目录中没有任何软件包，只有一个CMakeLists.txt链接文件），你依然可以编译它：

$ cd ~/catkin_ws/
$ catkin_make

catkin_make命令在catkin 工作空间中是一个非常方便的工具。如果你查看一下当前目录应该能看到’build’和’devel’这两个文件夹。在’devel’文件夹里面你可以看到几个setup.sh文件。source这些文件中的任何一个都可以将当前工作空间设置在ROS工作环境的最顶层，想了解更多请参考catkin文档。接下来首先source一下新生成的setup.sh文件：

$ source devel/setup.bash

要想保证工作空间已配置正确需确保ROS_PACKAGE_PATH环境变量包含你的工作空间目录，采用以下命令查看：

$ echo $ROS_PACKAGE_PATH

到此你的工作环境已经搭建完成，接下来可以继续学习 ROS文件系统教程.
每次打开终端自动加载ROS环境和工作空间

$sudo gedit ~/.bashrc

添加内容到文件末尾

source /home/wpr/catkin_test/devel/setup.bash

构建Catkin包

下面内容为如何使用catkin_create_pkg命令来创建一个新的catkin程序包以及创建之后都能做些什么。

$ cd ~/catkin_ws/src

现在使用catkin_create_pkg命令来创建一个名为’beginner_tutorials’的新程序包，这个程序包依赖于std_msgs、roscpp和rospy：

$ catkin_create_pkg beginner_tutorials std_msgs rospy roscpp

这将会创建一个名为beginner_tutorials的文件夹，这个文件夹里面包含一个package.xml文件和一个CMakeLists.txt文件，这两个文件都已经自动包含了部分你在执行catkin_create_pkg命令时提供的信息。
catkin_create_pkg命令会要求你输入package_name，如果有需要你还可以在后面添加一些需要依赖的其它程序包。
程序包依赖关系

$ rospack depends1 beginner_tutorials

~/catkin_test/src$ rospack depends1 beginner_tutorials 
roscpp
rospy
std_msgs

就像你看到的，rospack列出了在运行catkin_create_pkg命令时作为参数的依赖包，这些依赖包随后保存在package.xml文件中。

$ roscd beginner_tutorials
$ cat package.xml

<?xml version="1.0"?>
<package>
...
 <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>std_msgs</run_depend>

...
</package>

搭建开发环境(catkin_make)

编译包：

$ cd ~/catkin_test
$ catkin_make

输出结果：



Base path: /home/user/catkin_ws
Source space: /home/user/catkin_ws/src
Build space: /home/user/catkin_ws/build
Devel space: /home/user/catkin_ws/devel
Install space: /home/user/catkin_ws/install
####
#### Running command: "cmake /home/user/catkin_ws/src
-DCATKIN_DEVEL_PREFIX=/home/user/catkin_ws/devel
-DCMAKE_INSTALL_PREFIX=/home/user/catkin_ws/install" in "/home/user/catkin_ws/build"
####
-- The C compiler identification is GNU 4.2.1
-- The CXX compiler identification is Clang 4.0.0
-- Checking whether C compiler has -isysroot
-- Checking whether C compiler has -isysroot - yes
-- Checking whether C compiler supports OSX deployment target flag
-- Checking whether C compiler supports OSX deployment target flag - yes
-- Check for working C compiler: /usr/bin/gcc
-- Check for working C compiler: /usr/bin/gcc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Using CATKIN_DEVEL_PREFIX: /tmp/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /opt/ros/groovy
-- This workspace overlays: /opt/ros/groovy
-- Found PythonInterp: /usr/bin/python (found version "2.7.1") 
-- Found PY_em: /usr/lib/python2.7/dist-packages/em.pyc
-- Found gtest: gtests will be built
-- catkin 0.5.51
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~  traversing packages in topological order:
-- ~~  - beginner_tutorials
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ add_subdirectory(beginner_tutorials)
-- Configuring done
-- Generating done
-- Build files have been written to: /home/user/catkin_ws/build
####
#### Running command: "make -j4" in "/home/user/catkin_ws/build"
####

编写简单发布节点和订阅节点

参考：https://www.ncnynl.com/archives/201701/1279.html
介绍用C++编写简单发布节点和订阅节点

$ roscd beginner_tutorials
$ mkdir src
$ touch src/talker.cpp
$ vim src/talker.cpp

talker.cpp 内容：

#include "ros/ros.h" //导入ros包核心公共头文件
#include "std_msgs/String.h"//导入消息头文件

#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.
   */
  ros::init(argc, argv, "talker");//初始化ros，指定节点名称为talker，节点名称保持唯一性。

  /**
   * 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 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.
   */
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
//发布一个消息类型为std_msg/string,名称为chatter的话题。定义消息队列大小为1000，超过1000条消息后会丢弃。
  ros::Rate loop_rate(10);//指定发布消息的频率，这里指10Hz  通过 Rate::sleep()来处理睡眠的时间来控制对应的发布频率。

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

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();
//实例化消息msg, 定义字符串流“hello world”并赋值给ss, 最后转成为字符串赋值给msg.data
    ROS_INFO("%s", msg.data.c_str());
//输出调试信息
    /**
     * 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.
     */
    chatter_pub.publish(msg);
//实际发布的函数
    ros::spinOnce();

    loop_rate.sleep();//根据之前ros::Rate loop_rate(10)的定义来控制发布话题的频率。定义10即为每秒10次
    ++count;
  }


  return 0;
}

listener.cpp

#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;
}

在之前使用catkin_create_pkg创建包beginner_tutorials，会得到 package.xml 和 CMakeLists.txt两个文件
增加如下代码到CMakeLists.txt：

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)

进入工作空间编译

$ cd ~/catkin_test
$ catkin_make

测试：

roscore

rosrun beginner_tutorials talker
rosrun beginner_tutorials listener

编写简单服务端和客户端

添加srv服务

$ mkdir srv

这次我们不再手动创建服务，而是从其他的package中复制一个服务。 roscp是一个很实用的命令行工具，它实现了将文件从一个package复制到另外一个package的功能。

$ roscp rospy_tutorials AddTwoInts.srv srv/AddTwoInts.srv

还有很关键的一步：我们要确保srv文件被转换成C++，Python和其他语言的源代码。
在CMakeLists.txt文件中增加了对message_generation的依赖。:

# Do not just add this line to your CMakeLists.txt, modify the existing line
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation)

删掉#，去除对下边语句的注释:

# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

用你自己的srv文件名替换掉那些Service*.srv文件:

add_service_files(
  FILES
  AddTwoInts.srv
)

接下来，在CMakeLists.txt中找到如下部分:

# generate_messages(
#   DEPENDENCIES
# #  std_msgs  # Or other packages containing msgs
# )

去掉注释并附加上所有你消息文件所依赖的那些含有.msg文件的package（这个例子是依赖std_msgs,不要添加roscpp,rospy)，结果如下:

generate_messages(
  DEPENDENCIES
  std_msgs
)

现在，你可以生成自己的服务源代码了。

接下来介绍c++编写简单的服务端节点和客户端节点
创建”add_two_ints_server”服务端，接收两个数字，返回两个值的和。

编写服务端节点

cd ~/catkin_test/src/beginner_tutorials

创建src/add_two_ints_server.cpp文件，并复制粘贴下面的代码：

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"

bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::AddTwoInts::Response &res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("add_two_ints", add);
  ROS_INFO("Ready to add two ints.");
  ros::spin();

  return 0;
}

添加客户端程序：

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <cstdlib>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_client");
  if (argc != 3)
  {
    ROS_INFO("usage: add_two_ints_client X Y");
    return 1;
  }

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
  beginner_tutorials::AddTwoInts srv;
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);
  if (client.call(srv))
  {
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
  }

  return 0;
}

测试效果：
新开一个终端输入

$ rosrun beginner_tutorials add_two_ints_server

新开一个终端输入

$ rosrun beginner_tutorials add_two_ints_client 2 4

可以看到返回：

[ INFO] [1536240568.268034727]: Sum: 6

使用类方法作为回调函数

大多数教程使用函数的例子，而不是类方法。这是因为使用函数比较简单，不是因为不支持类方法。
下面展示如何使用类方法订阅服务回调。

订阅端

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

/**
 * This tutorial demonstrates subscribing to a topic using a class method as the callback.
 */

// %Tag(CLASS_WITH_DECLARATION)%
class Listener
{
public:
  void callback(const std_msgs::String::ConstPtr& msg);
};
// %EndTag(CLASS_WITH_DECLARATION)%

void Listener::callback(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, "listener_class");
  ros::NodeHandle n;

// %Tag(SUBSCRIBER)%
  Listener listener;
  ros::Subscriber sub = n.subscribe("chatter", 1000, &Listener::callback, &listener);
// %EndTag(SUBSCRIBER)%

  ros::spin();

  return 0;
}

服务端

#include "ros/ros.h"
#include "roscpp_tutorials/TwoInts.h"

// %Tag(CLASS_DECLARATION)%
class AddTwo
{
public:
  bool add(beginner_tutorials::TwoInts::Request& req,
           beginner_tutorials::TwoInts::Response& res);
};
// %EndTag(CLASS_DECLARATION)%

bool AddTwo::add(beginner_tutorials::TwoInts::Request& req,
                 beginner_tutorials::TwoInts::Response& res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("  sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;

// %Tag(SERVICE_SERVER)%
  AddTwo a;
  ros::ServiceServer ss = n.advertiseService("add_two_ints", &AddTwo::add, &a);
// %EndTag(SERVICE_SERVER)%

  ros::spin();

  return 0;
}

使用Timers类

新建timer_test.cpp

#include "ros/ros.h"

/**
 * This tutorial demonstrates the use of timer callbacks.
 */

void callback1(const ros::TimerEvent&)
{
  ROS_INFO("Callback 1 triggered");
}

void callback2(const ros::TimerEvent&)
{
  ROS_INFO("Callback 2 triggered");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;

  /**
   * Timers allow you to get a callback at a specified rate.  Here we create
   * two timers at different rates as a demonstration.
   */
  ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);
  ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);

  ros::spin();

  return 0;
}

在CMakeLists.txt中添加

add_executable(timer_test src/timer_test.cpp)
target_link_libraries(timer_test ${catkin_LIBRARIES})
add_dependencies(timer_test beginner_tutorials_generate_messages_cpp)

编写高级的发布器和订阅器

描述如何创建发布节点：
针对launch文件或命令行，初始化几个变量参数来使用参数服务器
通过使用动态配置服务器，在运行时修改参数来调整
使用自定义消息来发布话题
创建一个订阅节点与发布节点一起工作：
设置订阅节点以侦听指定主题上的自定义消息并打印出消息数据

Callbacks和Spinning

Callbacks(回调函数)
roscpp使用线程在后台做网络管理、调度等，它永远不会暴露线程给应用程序。
所以roscpp可以做的，就是允许你的回调函数调用任意数量的线程。
但是如果我们想调用这个回调函数，就需要写一些其他的程序。否则订阅、服务等回调将永远不会被调用。
最常见的解决方案是ros::spin()。
注意：回调队列Spinning对roscpp内部网络通信没有影响，他们只会在回调发生时影响用户。他们影响订阅队列的效果，因为多久开始处理回调和回调函数处理完的时间决定着消息是否会被丢弃。
单线程Spinning

ros::init(argc, argv, "my_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe(...);
...
ros::spin();

在这个程序中，用户的回调函数在ros::spin()中被调用。
ros::spin()在节点关闭或ros::shutdown()或按下Ctrl-C才会返回
另一个常见的模式是定期运行ros::spinonce()：

ros::Rate r(10); // 10 hz
while (should_continue)
{
  ... do some work, publish some messages, etc. ...
  ros::spinOnce();
  r.sleep();
}

ros::spinOnce()在那个时间点，会调用所有的回调函数。
执行一个自己的spin()函数很简单：

#include <ros/callback_queue.h>
ros::NodeHandle n;
while (ros::ok())
{
  ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1));
}

注：spin()和spinonce()真的意味着单线程应用程序，而不是从多个线程中调用的一次优化。

多线程Spinning
roscpp提供了一些内建方法来使用多线程的回调函数。
有两个内建的方法：ros::MultiThreadedSpinner、ros::AsyncSpinner (since 0.10)
ros::MultiThreadedSpinner
MultiThreadedSpinner是阻塞式的spinner，类似ros::spin()。

ros::MultiThreadedSpinner spinner(4); // Use 4 threads
spinner.spin(); // spin() will not return until the node has been shutdown

ros::AsyncSpinner (since 0.10)
很有用的线程spinner是AsyncSpinner，它不是阻塞式的spin()调用
它有start() 和stop()函数，当它销毁时候就会自动关闭：

ros::AsyncSpinner spinner(4); // Use 4 threads
spinner.start();
ros::waitForShutdown();

ROS与C++学习1