ROS学习之cpp教程

官方链接：

    http://wiki.ros.org/roscpp_tutorials

一、编写简单的消息发布器和订阅器

    

    1、编写一个发布器节点

        节点(node)是ROS中值代连接到ros网络中的可执行文件。接下来，将会创建一个发布器节点（talker）,

        他将不断在ROS网络中广播消息。

        转移到～目录下创建一个catkin工作空间，在src目录下创建一个包，在包目录下创建一个src目录,创建*.cpp文件，编写代码

        

        常用的 ros/ros.h 引用ros系统中大部分常用的头文件

        std_msgs/String.h 引用标准消息包下的String消息

        ros::init(argc,argv,"node_name");//初始化ROS,允许ROS通过命令行进行名称重映射。这里指定唯一的节点名称

        ros::NodeHandle n; //为这个进程的节点创建一个句柄,第一个被创建的句柄会为节点进行初始化,最后一个被销毁的句柄会清理节点使用的所有资源

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

        //告诉master节点我们将要在名为chatter的话题上发布一个std_msgs/String类型的消息.这样master就会告知所有订阅了chatter话题的节点,将要有数据消息被发布.第二个参数1000指发布序列的大小.如果缓冲区中的消息数目大于1000,就会丢弃之前的消息.

        NodeHandle::advertise()函数会返回一个ros::Publisher对象,它的publish(msg)成员函数可以让你在topic话题上发布消息,如果消息类型与advertise<msgs_type>("topic_name",num);中的类型不对应,那么将会拒绝发布消息.

        ros::Rate 对象可以允许制定子循环的频率,它会追踪记录自上一次调用Rate::sleep()之后的时间流逝,并休眠直到一个频率周期的时间.

        ros::ok()函数会返回false当下述情况出现时:SIGINT(^C) 被另一个同名节点踢出ROS网络 ros::shutdown()函数被该程序的另一部分调用  

        std_msgs::String msg; 定义消息对象,

        msg.data = ss.str(); 填充消息对象成员,

        chatter_pub.publish(msg); 发布消息

        ROS_INFO("%s",string);用于代替printf/cout向控制台界面发布显示信息

        ros::spinOnce()函数,允许接受回调函数,spin是旋转,停留的意思

        总结一下: 1.初始化ROS系统  

             2.在ROS网络内广播我们将在chatter topic上发布std_msgs/String消息

             3.以每秒10次的频率在chatter上发布消息

    2、编写一个订阅器节点

        chatterCallback()回调函数会处理订阅的主题的消息.

        NodeHandle对象的subscribe()函数设置订阅的主题名,队列的大小,设置消息到来时的回调函数,返回一个ros::Subscriber对象

        当ros::Subscriber对象被销毁时,将自动退订主题上面的消息

        还有不同的NodeHandle::subscribe()函数,允许指定类的成员函数,具体查看roscpp overview(http://wiki.ros.org/roscpp/Overview)

        ros::spin()函数进入自循环,可以尽可能快的调用消息回调函数.

        总结一下: 1.初始化ROS系统  

              2.订阅chatter topic主题   

              3.进入自循环,等待消息的到达

              4.当消息到达时,调用chatterCallback()函数

    3.编译工作空间,对修改过的程序包进行编译

        1)修改程序包目录下的CMakeLists.txt文件,大体上类似下面

            cmake_minimum_required(VERSION 2.8.3)      #需要的cmake的最小版本号

            project(m_pkg)                   #程序包名

            ## Find catkin and any catkin packages     #需要的其他包

            find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)

            ## Declare ROS messages and services       #添加消息文件

            add_message_files(FILES String.msg)

            #add_service_files(FILES AddTwoInts.srv)

            ## Generate added messages and services    #生成消息文件

            generate_messages(DEPENDENCIES std_msgs)

            ## Declare a catkin package

            catkin_package()

            ## Build talker and listener

            include_directories(include ${catkin_INCLUDE_DIRS})

            add_executable(talker src/talker.cpp)

            target_link_libraries(talker ${catkin_LIBRARIES})

            add_dependencies(talker m_pkg_generate_messages_cpp)

            add_executable(listener src/listener.cpp)

            target_link_libraries(listener ${catkin_LIBRARIES})

            add_dependencies(listener m_pkg_generate_messages_cpp)

        2)进入工作空间目录,执行catkin_make编译,显示如下:

            Base path: /home/sl/m_ros_ws             #工作空间目录

            Source space: /home/sl/m_ros_ws/src        #源代码src目录

            Build space: /home/sl/m_ros_ws/build        #build目录

            Devel space: /home/sl/m_ros_ws/devel        #devel目录

            Install space: /home/sl/m_ros_ws/install    #install目录(如果有的话)

            ####

            #### Running command: "make cmake_check_build_system" in "/home/sl/m_ros_ws/build"

            ####

            -- Using CATKIN_DEVEL_PREFIX: /home/sl/m_ros_ws/devel

            -- Using CMAKE_PREFIX_PATH: /home/sl/m_ros_ws/devel;/opt/ros/jade

            -- This workspace overlays: /home/sl/m_ros_ws/devel;/opt/ros/jade

            -- Using PYTHON_EXECUTABLE: /usr/bin/python

            -- Using Debian Python package layout

            -- Using empy: /usr/bin/empy

            -- Using CATKIN_ENABLE_TESTING: ON

            -- Call enable_testing()

            -- Using CATKIN_TEST_RESULTS_DIR: /home/sl/m_ros_ws/build/test_results

            -- Found gtest sources under '/usr/src/gtest': gtests will be built

            -- Using Python nosetests: /usr/bin/nosetests-2.7

            -- catkin 0.6.16

            -- BUILD_SHARED_LIBS is on

            -- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

            -- ~~  traversing 1 packages in topological order:    #只定位到1个程序包

            -- ~~  - m_pkg

            -- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

            -- +++ processing catkin package: 'm_pkg'           #处理这个catkin程序包

            -- ==> add_subdirectory(m_pkg)

            -- Using these message generators: gencpp;geneus;genlisp;genpy

            -- m_pkg: 1 messages, 0 services              #显示只有1个消息,0个服务

            -- Configuring done                    #配置完成

            -- Generating done                    #生成完成

            -- Build files have been written to: /home/sl/m_ros_ws/build #编译文件写入工作空间build目录

            ####

            #### Running command: "make -j4 -l4" in "/home/sl/m_ros_ws/build"  #开始编译

            ####

            [  0%] Built target std_msgs_generate_messages_eus

            [  0%] [  0%] Built target std_msgs_generate_messages_cpp

            Built target std_msgs_generate_messages_py

            [  0%] Built target std_msgs_generate_messages_lisp

            [  0%] Built target _m_pkg_generate_messages_check_deps_String

            [ 62%] [ 62%] [ 62%] Built target m_pkg_generate_messages_lisp

            Built target m_pkg_generate_messages_py

            Built target m_pkg_generate_messages_eus

            [ 75%] Built target m_pkg_generate_messages_cpp

            [ 75%] Built target m_pkg_generate_messages

            [ 87%] [100%] Built target listener

            Built target talker

        这会产生两个可执行文件talker和listener默认存储在devel目录下(具体在~/m_ros_ws/devel/lib/<pkg name>中)

    4.运行节点

        1)启动节点命名管理器master节点(主节点)

            开启一个terminal,输入roscore   如果有错误,执行 source /opt/ros/<ros-distr>/setup.bash定位一下

        2)运行listener节点

            开启一个终端,定位下工作空间(如果写入~/.bashrc,可省略此步) 执行source ~/m_ros_ws/devel/setup.bash

            然后,执行 rosrun m_pkg listener 运行节点

        3)运行talker节点

            开启另一个终端,定位下工作空间(如果写入~/.bashrc,可省略此步) 执行source ~/m_ros_ws/devel/setup.bash

            然后,执行 rosrun m_pkg talker 运行节点

                Talker将会输出如下类似文字:

                [ INFO] I published [Hello there! This is message [0]]

                [ INFO] I published [Hello there! This is message [1]]

                [ INFO] I published [Hello there! This is message [2]]

                ...

                同时listener也会开始输出类似的文本信息：

                [ INFO] Received [Hello there! This is message [20]]

                [ INFO] Received [Hello there! This is message [21]]

                [ INFO] Received [Hello there! This is message [22]]

                ...

    

二、编写简单的Service和Client

    1.编写简单的Service节点

        在程序包中的srv目录下创建服务文件*.srv  具体参见：http://wiki.ros.org/ROS/Tutorials/CreatingMsgAndSrv#Creating_a_srv

        在程序包中的src目录下创建add_two_ints_server.cpp,编写代码

    2.编写简单的client节点

        在程序包的src目录下创建add_two_ints_client.cpp，编写代码

    3.编译节点

        打开程序包的CMakeLists.txt文件，在后面添加如下几行：

            add_executable(add_two_ints_server src/add_two_ints_server.cpp)

            target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})

            add_dependencies(add_two_ints_server m_pkg_gencpp)

            add_executable(add_two_ints_client src/add_two_ints_client.cpp)

            target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})

            add_dependencies(add_two_ints_client m_pkg_gencpp)

        这几行将控制生成两个可执行程序add_two_ints_server和add_two_ints_client,这两个可执行程序被放在devel/lib/share/m_pkg下

            

        在工作空间执行catkin_make,显示

            Scanning dependencies of target add_two_ints_client

            [ 52%] [ 78%] Built target m_pkg_generate_messages_eus

            Built target m_pkg_generate_messages_py

            Scanning dependencies of target add_two_ints_server

            [ 84%] [ 89%] Built target listener

            Built target talker

            [ 89%] Built target m_pkg_generate_messages

            [100%] [100%] Building CXX object m_pkg/CMakeFiles/add_two_ints_server.dir/src/add_two_ints_server.cpp.o

            Building CXX object m_pkg/CMakeFiles/add_two_ints_client.dir/src/add_two_ints_client.cpp.o

            Linking CXX executable /home/sl/m_ros_ws/devel/lib/m_pkg/add_two_ints_client

            Linking CXX executable /home/sl/m_ros_ws/devel/lib/m_pkg/add_two_ints_server

            [100%] Built target add_two_ints_client

            [100%] Built target add_two_ints_server

三、使用参数  

wiki: http://wiki.ros.org/cn/roscpp_tutorials/Tutorials/Parameters

    1.提取参数

        使用NodeHandle有两种方法提取参数。在下面的例子中，n代表一个节点句柄对象。

        1）getParam()函数   std::string s;

                    n.getParam("my_param", s);    //key+value  类似键值对

        2）param()函数        int i;

                      n.param("my_num", i, 42);  //如果不成功，对变量i赋默认值42

    2.设置参数

        使用setparam()函数  n.setParam("my_param", "hello there");

    3.删除参数

        使用deleteParam()函数 n.deleteParam("my_param");

    4.判断参数存在

        使用hasParam()函数 hasParam("my_param")，存在返回ture,不存在返回false

    5.寻找参数

        参数服务器使你能够查询参数从你的命名空间开始，通过你的父命名空间向上找。类似于树搜索

        searchParam("search_name",param_name)函数

四、从节点句柄获取私有名字

wiki: http://wiki.ros.org/cn/roscpp_tutorials/Tutorials/AccessingPrivateNamesWithNodeHandle
    如果直接使用节点句柄自己的名字空间，可能会引起混乱。

    在创建节点句柄时，应使用私有名称参数传递。

        例如：

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

            ros::NodeHandle nh1("~");        //nh1的名字空间是/my_node_name

            ros::NodeHandle nh2("~foo");        //nh2的名字空间是/my_node_name/foo

    

五、使用类成员方法做回调函数

wiki : http://wiki.ros.org/cn/roscpp_tutorials/Tutorials/UsingClassMethodsAsCallbacks
    除了使用自己定义的函数作为回调函数之外，ROS支持使用类的成员方法作为回调函数

    1.订阅者回调

    使用一般的自定义函数chatterCallback作回调：

        ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

    使用类的成员方法作回调：

        class Listener

        {

        public:

          void callback(const std_msgs::String::ConstPtr& msg);

        };

        Listener listener;  //实例化类对象

        ros::Subscriber sub = n.subscribe("chatter", 1000, &Listener::callback, &listener);

    2.服务服务器(Service Servers)

        

    使用自定义的回调函数：

    ros::ServiceServer service = n.advertiseService("add_two_ints", add);

    使用类的成员方法：

    class AddTwo

    {

    public:

      bool add(roscpp_tutorials::TwoInts::Request& req,

           roscpp_tutorials::TwoInts::Response& res);

    };

    AddTwo a;

    ros::ServiceServer ss = n.advertiseService("add_two_ints", &AddTwo::add, &a);

六、定时器的使用

wiki: http://wiki.ros.org/roscpp_tutorials/Tutorials/Timers
    描述：定时器Timer允许周期性地调用一个回调函数。它们比ros::Rate类更加灵活有用。

    注意，定时器Timer类不是实时线程/内核代替，对于精度没有保障。会因系统负载/容量发生很大变化。

    1.使用定时器基础知识

    实例化定时器对象与Subscriber对象类似： ros::Timer timer = n.createTimer(ros::Duration(0.1),timerCallback);

    定时器回调的形式：void timerCallback(const ros::TimerEvent& e);

    

    时间事件结构体: ros::TimerEvent

    

    2.多定时器例子

        创建两个定时器

            ros::Timer timer1 = n.createTimer(ros::Duration(0.1), callback1);   //100 ms

            ros::Timer timer2 = n.createTimer(ros::Duration(1.0), callback2);   //1 second

        sl@sl-desktop:~/m_ros_ws$ rosrun m_pkg timer_test

        [ INFO] [1468647866.507138304]: Callback 1 triggered

        [ INFO] [1468647866.607406741]: Callback 1 triggered

        [ INFO] [1468647866.707729591]: Callback 1 triggered

        [ INFO] [1468647866.806902543]: Callback 1 triggered

        [ INFO] [1468647866.907178881]: Callback 1 triggered

        [ INFO] [1468647867.007414974]: Callback 1 triggered

        [ INFO] [1468647867.107685959]: Callback 1 triggered

        [ INFO] [1468647867.206911026]: Callback 1 triggered

        [ INFO] [1468647867.307240687]: Callback 1 triggered

        [ INFO] [1468647867.407530667]: Callback 1 triggered

        [ INFO] [1468647867.407588676]: Callback 2 triggered

        [ INFO] [1468647867.507788676]: Callback 1 triggered

        [ INFO] [1468647867.607068827]: Callback 1 triggered

        [ INFO] [1468647867.707315687]: Callback 1 triggered

        [ INFO] [1468647867.807539675]: Callback 1 triggered

        [ INFO] [1468647867.907822600]: Callback 1 triggered

        [ INFO] [1468647868.007130929]: Callback 1 triggered

        [ INFO] [1468647868.107539634]: Callback 1 triggered

        [ INFO] [1468647868.207966415]: Callback 1 triggered