ROS1（四）：ROS1开发

26 August, 2022. It was a Friday.

view:

ROS1开发

ROS1开发

创建工作空间与功能包

工作空间是一个存放工程开发相关文件的文件夹：

src：代码空间
build：编译空间
devel：开发空间
install：安装空间

在对应的文件夹下执行：

catkin_init_workspace

编译项目：

catkin_make
catkin_make install # 生成安装空间

在 src 目录下创建对应的功能包：

catkin_create_pkg test_pkg std_msgs rospy roscpp

编译功能包：

在工作空间的根目录下

catkin_make

source devel/setup.bash

Topic编程实现

发布者Publish编程实现

步骤：

初始化ros节点
向ros master注册节点信息，包括发布的话题名和话题中的消息类型
创建消息数据
按照一定频率循环发送消息

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char **argv){
    // ros节点初始化
    ros::init(argc, argv, "velocity_publisher")

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为 geometry_msgs::Twist，队列长度为10
    ros::Publish turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);

    // 设置循环的频率
    ros::Rate loop_rate(10);

    int count = 0;
    while (ros::ok()) {
        // 初始化 geometry_msgs::Twist 类型的消息
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5;
        vel_msg.angular.z = 0.2;

        // 发布消息
        turtle_vel_pub.publish(vel_msg);
        ROS_INFO("Publish turtle velocity command[%0.2f m/s,%0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z);

        // 按照循环频率延时
        loop_rate.sleep();
    }
    return 0;

在 CMakeList.txt 文件中增加如下的内容：

add_executable(velocity_publisher src/velocity_publisher.cpp)
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

然后执行编译(在工作空间下)：

catkin_make

source devel/setup.bash

运行：

roscore

rosrun turtlesim turtlesim_node
rosrun learning_topic velocity_publisher

订阅者Subscriber编程实现

实现步骤：

初始化ROS节点
订阅需要的话题
循环等待话题消息，接收到消息后进入回调函数
在回调函数中完成消息处理

#include <ros/ros.h>
#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg){
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f,y:%0.6f", msg->x,msg->y);
}

int main(int argc, char **argv){
    // 初始化ros节点
    ros::init(argc, argv, "pose_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

在 CMakeList.txt 文件中增加如下的内容：

add_executable(pose_subscriber src/pose_subscriber.cpp)
target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

然后执行编译(在工作空间下)：

catkin_make

source devel/setup.bash

运行：

roscore

rosrun turtlesim turtlesim_node
rosrun learning_topic velocity_publisher
rosrun learning_topic pose_subscriber

话题消息的定义和使用

话题中传输的消息是可以自定义的，定义的步骤如下：

定义 msg 文件
在package.xml中添加功能包依赖
在CMakeList.txt中添加编译选项
编译生成语言相关文件

比如定义一个 person 的相关信息：

1、首先定义一个文件：Person.msg

string name
uint8 sex
uint8 age

uint8 unknown = 0
uint8 male = 1
uint8 female = 2

2、在package.xml中添加功能包依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

3、在CMakeList.txt中添加编译选项

find_package(
    ...... 
    message_generation
)

add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)

catkin_package(
    ...... 
    message_runtime
)

4、编译生成语言相关文件

catkin_make

编译完成之后会在 include 中生成对应的 Person.h 文件。

使用自定义的 msg 来完成一个简单的 Publish/Subscribe

Publisher:

#include <ros/ros.h>
#include <learning_topic/Person.h>

int main(int argc, char **argv){
    // ros节点初始化
    ros::init(argc, argv, "person_publisher")

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Publisher，发布名为/person_info的topic，消息类型为 learning_topic::Person，队列长度为10
    ros::Publish person_info_pub = n.advertise<learning_topic::Person>("/person_info",10);

    // 设置循环的频率
    ros::Rate loop_rate(10);

    int count = 0;
    while (ros::ok()) {
        // 初始化 geometry_msgs::Twist 类型的消息
        learning_topic::Person person_msg;
        person_msg.name = "zhangsan";
        person_msg.age = 10;
        person_msg.sex = learning_topic::Person::male;

        // 发布消息
        person_info_pub.publish(person_msg);
        ROS_INFO("Publish Person info: name:%s,age:%d,sex:%d",person_msg.name,person_msg.age,person_msg.sex);

        // 按照循环频率延时
        loop_rate.sleep();
    }
    return 0;

Subscriber:

#include <ros/ros.h>
#include <learning_topic/Person.h>

// 接收到订阅的消息后，会进入消息回调函数
void personCallback(const learning_topic::Person::ConstPtr& msg){
    // 将接收到的消息打印出来
    ROS_INFO("Subscribe Person info: name:%s,age:%d,sex:%d",msg->name,msg->age,msg->sex);
}

int main(int argc, char **argv){
    // 初始化ros节点
    ros::init(argc, argv, "person_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个subscriber，订阅名为/person_info的topic，注册回调函数poseCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

使用自定义消息进行 Publish 和 Subscribe 的时候要在 CMakeList.txt 文件中额外新增如下的配置(publish和subscribe都需要增加，其他配置和正常publish/subscribe相同)：

add_executable(person_publisher src/person_publisher.cpp)
target_link_libraries(person_publisher ${catkin_LIBRARIES})
# 新增
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)


add_executable(person_subscriber src/person_subscriber.cpp)
target_link_libraries(person_subscriber ${catkin_LIBRARIES})
# 新增
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

编译运行：

catkin_make

source devel/setup.bash

roscore
rosrun learning_topic person_publisher
rosrun learning_topic person_subscriber

Service编程实现

Client客户端编程实现

首先在工作空间下创建对应的功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

创建一个客户端的步骤：

初始化 ros 节点；
创建一个 Client 实例；
发布服务器请求数据；
等待Server处理之后的应答结果；

接下来使用代码实现一个 client 来创建一个 turtlesim，对应 client 的编程实现：

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

int main(int argc, char **argv){
    // 初始化ros节点
    ros::init(argc, argv, "turtle_spawn");

    // 创建节点句柄
    ros::NodeHandle node;

    // 发现 /spawn 服务后(阻塞函数)，创建一个服务客户端，连接名为/spawn的server
    ros::service::waitForService("/spawn");
    ros::serviceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");

    // 初始化 turtlesim 的请求数据
    turtlesim::Spawn srv;
    srv.request.x = 2.0;
    srv.request.y = 2.0;
    srv.request.name = "turtle2";

    // 请求server
    ROS_INFO("call server to spawn turtle[x:%0.6f,y:%0.6f,name:%s]",srv.request.x,srv.request.y,srv.request.name.c_str());
    add_turtle.call(srv);

    // 处理响应数据
    ROS_INFO("call spawn server successfully, response is %s",srv.response.name.c_str());

    return 0;
}

在 CMakeList.txt 中增加对应server的编译规则

add_executable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

开始进行编译运行：

cd ~/catkin_ws

catkin_make

source devel/setup.bash

roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_spawn

Server服务端编程实现

如何实现一个 server：

初始化ROS节点；
创建Server实例；
循环等待服务请求，进入回调函数；
在回调函数中完成服务请求的处理，并反馈应答数据；

使用 Server 服务端来完成如下的任务：

Server 通过接收 Client 发送的请求来向 /turtle_command Service 发送指令来让 turtle 进行移动或停止，然后给 Client 进行响应。

turtle_command_server.cpp:

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>

ros::Publisher turtle_vel_pub;
bool pubCommand = false;

// service回调函数，传入参数req，输出参数res
bool commandCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){
    pubCommand = !pubCommand;

    // 显示请求数据
    ROS_INFO("Publish turtle velocity command [%s]", pubCommand == true?"Yes":"No");

    // 设置反馈数据
    res.success = true;
    res.message = "Change turtle command state!";

    return true;
}

int main(int argc, char **argv){
    // ros节点初始化
    ros::init(argc, argv, "turtle_command_server");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个名为/turtle_command的server，注册回调函数 commandCallback
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

    // 创建一个Publish发布者，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度为10
    turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);

    // 循环等待回调函数
    ROS_INFO("Ready to receive turtle command.");

    // 设置循环的频率
    ros::Rate loop_rate(10);

    while(ros::ok()){
        // 查看一次回调函数队列
        ros::spinOnce();

        // 如果标志为true，则发布速度指令
        if(pubCommand){
            geometry_msgs::Twist vel_msg;
            vel_msg.linear.x = 0.5;
            vel_msg.angular.z = 0.2;
            turtle_vel_pub.publish(vel_msg);
        }

        // 按照循环频率延时
        loop_rate.sleep();
    }

    return 0;

修改 CMakeLists.txt 文件：

add_executable(turtle_command_server src/turtle_command_server.cpp)
target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

编译运行：

cd ~/catkin_ws

catkin_make

source devel/setup.bash

roscore
rosrun turtlesim turtlesim_node
rosrun learning_service turtle_command_server
rosservice call /turtle_command "{}"

Service数据的定义和使用

上面的示例中使用的是 Service 中已经定义好的 Spawn Trigger 两种服务数据，现在我们自己来定义一个服务数据来进行通信。

因此我们使用 server 来实现一个传输 Person 信息的功能，设计如下：

自定义服务数据的步骤：

1、定义 srv 文件（---上面是请求格式，下面是响应格式）

string name
uint8 age
uint8 sex

uint8 unknown = 0
uint8 male = 1
uint8 female = 2
---
string result

2、在package.xml中添加功能依赖包

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

3、在CMakeLists.txt中增加编译选项

find_package(
    ...
    message_generation
)

add_service_files(FILES Person.srv)
generate_messages(DEPENDENCIES std_msgs)

catkin_package(
    ...
    message_runtime
)

4、编译生成语言相关文件

catkin_make

定义好自定义server数据之后就可以在程序中进行使用了：

服务端(person_server.cpp)：

#include <ros/ros.h>
#include <learning_service/Person.h>

// service回调函数，传入参数req，输出参数res
bool commandCallback(learning_service::Person::Request &req, learning_service::Person::Response &res){
    pubCommand = !pubCommand;

    // 显示请求数据
    ROS_INFO("Learning Server Person info is [name:%s,age:%d,sex:%d]", req.name.c_str(),req.age,req.sex);

    // 设置反馈数据
    res.result = "OK";

    return true;
}

int main(int argc, char **argv){
    // ros节点初始化
    ros::init(argc, argv, "person_server");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个名为show_person的server，注册回调函数 commandCallback
    ros::ServiceServer person_service = n.advertiseService("/show_person", commandCallback);

    // 循环等待回调函数
    ROS_INFO("Ready to show person information.");

    // 循环等待
    ros::spin();

    return 0;

客户端实现(person_client.cpp)：

#include <ros/ros.h>
#include <learning_service/Person.h>

int main(int argc, char **argv){
    // 初始化ros节点
    ros::init(argc, argv, "person_client");

    // 创建节点句柄
    ros::NodeHandle node;

    // 发现 /show_person 服务后(阻塞函数)，创建一个服务客户端，连接名为/show_person的server
    ros::service::waitForService("/show_person");
    ros::serviceClient person_client = node.serviceClient<learning_service::Person>("/show_person");

    // 初始化 turtlshow_personesim 的请求数据
    learning_service::Person srv;
    srv.request.name = "zhangsan";
    srv.request.age = 18;
    srv.request.sex = learning_service::Person::Request::male;

    // 请求server
    ROS_INFO("call server to show person:[name:%s,age:%d,sex:%d]",srv.request.c_str(),srv.request.age,srv.request.sex);
    add_turtle.call(srv);

    // 处理响应数据
    ROS_INFO("show person information response is %s",srv.response.result.c_str());

    return 0;
}

修改 CMakeLists.txt 文件：

add_executable(person_server src/person_server.cpp)
target_link_libraries(person_server ${catkin_LIBRARIES})
# 新增
add_dependencies(person_server ${PROJECT_NAME}_gencpp)

add_executable(person_client src/person_client.cpp)
target_link_libraries(person_client ${catkin_LIBRARIES})
# 新增
add_dependencies(person_client ${PROJECT_NAME}_gencpp)

编译运行：

cd ~/catkin_ws

catkin_make

source devel/setup.bash

roscore
rosrun learning_service person_server
rosrun learning_service person_client

Parameter参数编程实现

ROS master 中又一个对应的 parameter server(全局字典)，用来保存一些 node 所需要的参数信息：

ros 中关于 parameter 的相关命令：

列出当前所有的参数

rosparam list

现实某个参数值

rosparam get param_key

设置某个参数值

rosparam set param_key param_val

保存参数到文件

rosparam dump file_name（yaml参数文件）

从文件中读取参数

rosparam load file_name

删除参数

rosparam delete param_key

接下来通过编程来实现对应的 parameter server。

首先创建一个对应的功能包：

cd ~/catkin_ws/src

catkin_create_pkg learning_parameter roscpp rospy std_srvs

ros parameter 程序控制对应的步骤：

初始化ros节点
get函数获取参数
set函数设置参数

对应代码：parameter_config.cpp

#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>

int main(int argc, char **argv){
    int red,green,bule;

    // 初始化ros节点
    ros::init(argc,argv,"parameter_config");

    // 初始化node句柄
    ros::NodeHandle node;

    // get获取参数
    ros::param::get("/turtlesim/background_r", red);
    ros::param::get("/turtlesim/background_g", green);
    ros::param::get("/turtlesim/background_b", bule);
    ROS_INFO("Get Background Color[%d,%d,%d]",red,green,bule);

    // set设置参数
    ros::param::set("/turtlesim/backageground_r", 255);
    ros::param::set("/turtlesim/backageground_g", 255);
    ros::param::set("/turtlesim/backageground_b", 255);
    ROS_INFO("Set Background Color[255,255,255]");

    // get获取参数
    ros::param::get("/turtlesim/background_r", red);
    ros::param::get("/turtlesim/background_g", green);
    ros::param::get("/turtlesim/background_b", bule);
    ROS_INFO("Get Background Color[%d,%d,%d]",red,green,bule);

    // 调用 /clear 服务来刷新展示信息
    ros::service::waitForService("/clear");
    ros::serviceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
    std_srvs::Empty srv;
    clear_background.call(srv);

    sleep(1);

    return 0;
}

设置编译选项：

add_executable(parameter_config src/parameter_config.cpp)
target_link_libraries(parameter_config ${catkin_LIBRARIES})

编译运行：

cd ~/catkin_ws

catkin_make

source devel/setup.bash

roscore
rosrun turtlesim turtlesim_node
rosrun learning_parameter parameter_config