ROS 实践 -- 01 topic & service (c++)

1. 工作空间 workspace

1.1 Create Workspace

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace

1.2 Build Workspace

$ cd ~/catkin_ws
$ catkin_make

// 查看目录结构
$ tree -L 1 ~/catkin_ws
catkin_ws
├── build
├── devel
└── src

1.3 Init Workspace Env

$ source ~/catkin_ws/devel/setup.bash
// 查看工作空间环境变量（一层一层嵌套式的）
$ echo $ROS_PACKAGE_PATH
/home/pi/catkin_ws/src:/opt/ros/melodic/share

2. 自定义功能包 Package

catkin_create_pkg <package_name> [depend1] [depend2] [depend3]

// Create
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_communication std_msgs rospy roscpp

// Build
$ cd ~/catkin_w
$ catkin_make

// Env
$ source ~/catkin_ws/devel/setup.bash

创建自定义功能包 learning_communication 之后，可以查看一下里边的
package.xml

实际上rospack find、rosdep等命令之所以能快速定位和分析出package的依赖项信息，就是直接读取了每一个pacakge中的package.xml文件

package.xml 包含标签

3. 实现 Topic Demo

发布 / 订阅 publish / subscribe 通信模型

3.1 Create Publisher 发布者

源码：learning_communication/src/talker.cpp

#include <sstream>
#include "ros/ros.h"
#include "std_msgs/String.h"

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

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

    ros::Rate loop_rate(10);

    int count = 0;

    while (ros::ok())
    {
        std_msgs::String msg;
        std::stringstream ss;
        ss << "hello world" << count;
        msg.data = ss.str();
        ROS_INFO("%s", msg.data.c_str());
        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
    return 0;
}

3.2 Create Subscriber 订阅者

源码：learning_communication/src/listener.cpp

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

void chaterCallback(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");
    
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("chatter", 1000, chaterCallback);
    
    ros::spin();

    return 0;
}

3.3 编译

对于ROS这样大体量的平台来说，就采用的是CMake，并且ROS对CMake进行了扩展，于是便有了Catkin编译系统

3.3.1 编辑功能包里的 CMakeList.txt

如果使用了如 RoboWare Studio 支持ROS的IDE开发工具，会自动修改CMakeList.txt，可以跳过此步骤。
vi ~/catkin_ws/src/learning_communication/CMakeList.txt

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(talker src/talker.cpp)
add_dependencies(talker ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(talker ${catkin_LIBRARIES})

add_executable(listener src/listener.cpp) add_dependencies(listener ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(listener ${catkin_LIBRARIES})

3.3.2 编译 catkin_make

$ cd ~/catkin_ws
$ catkin_make

3.3.3 运行 Publisher 与 Subscriber

1) 初始化 Workspace 环境变量

$ source ~/catkin_ws/devel/setup.bash

2) 启动 roscore

$ roscore

3) 启动 roscore

$ rosrun learning_communication talker
[ INFO] [1613488268.472365199]: hello world0
[ INFO] [1613488268.572595116]: hello world1
[ INFO] [1613488268.672710021]: hello world2
[ INFO] [1613488268.772716550]: hello world3
[ INFO] [1613488268.872738656]: hello world4
[ INFO] [1613488268.972714741]: hello world5
[ INFO] [1613488269.072722907]: hello world6
...

4) 启动 roscore

$ rosrun learning_communication listener
[ INFO] [1613488311.557644751]: I heard: [hello world3]
[ INFO] [1613488311.657438007]: I heard: [hello world4]
[ INFO] [1613488311.757441799]: I heard: [hello world5]
[ INFO] [1613488311.857386007]: I heard: [hello world6]
[ INFO] [1613488311.957357331]: I heard: [hello world7]
[ INFO] [1613488312.057212225]: I heard: [hello world8]
[ INFO] [1613488312.157438854]: I heard: [hello world9]
[ INFO] [1613488312.257411530]: I heard: [hello world10]
...

4 实现 Service Demo

4.1 自定义服务数据

创建srv文件：learning_communication/srv/AddTowInts.srv
该文件包含请求与应答两个数据域，数据中的内容与话题消息数据类型相同，只是在请求与应答的描述之间，用“---”进行分割。

int64 a
int64 b
---
int64 sum

如果使用了如 RoboWare Studio 支持ROS的IDE开发工具，会自动修改 package.xml、CMakeList.txt，可以跳过此步骤。

package.xml
<build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>message_runtime</exec_depend>

CMakeLists.txt
find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs rospy roscpp)
add_service_files(FILES AddTowInts.srv)

4.2 创建 server

源文件：learning_communication/src/server.cpp

#include "ros/ros.h"
#include "learning_communication/AddTowInts.h"

bool add(learning_communication::AddTowInts::Request &req,
         learning_communication::AddTowInts::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[])
{
    /* code for main function */
    ros::init(argc, argv, "add_two_ints_server");
    
    ros::NodeHandle nh;

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

    ROS_INFO("Ready to add_two_ints.");
    ros::spin();

    return 0;
}

4.3 创建 Client

源码：learning_communimation/src/client.cpp

#include <cstdlib>
#include "ros/ros.h"
#include "learning_communication/AddTowInts.h"

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 nh;

    ros::ServiceClient client = nh.serviceClient<learning_communication::AddTowInts>("add_two_ints");

    learning_communication::AddTowInts 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;
}

4.4 编译功能包