通信机制案例

三个刺客

已于 2022-06-06 21:37:50 修改

阅读量537

点赞数

分类专栏： ROS 文章标签：自动驾驶 linux

于 2022-06-06 21:37:26 首次发布

本文链接：https://blog.csdn.net/mxh3600/article/details/125150288

版权

ROS 专栏收录该内容

23 篇文章 12 订阅

订阅专栏

学完三大通信机制后，要实践，话不多说，直接上项目！

项目一：话题发布

需求描述:编码实现乌龟运动控制，让小乌龟做圆周运动。

实现分析:

乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点 turtlesim_node，另一个是控制节点，二者是订阅发布模式实现通信的，乌龟运动显示节点直接调用即可，运动控制节点之前是使用的 turtle_teleop_key通过键盘控制，现在需要自定义控制节点。
控制节点自实现时，首先需要了解控制节点与显示节点通信使用的话题与消息，可以使用ros命令结合计算图来获取。
了解了话题与消息之后，通过 C++ 或 Python 编写运动控制节点，通过指定的话题，按照一定的逻辑发布消息即可。

实现流程:

通过计算图结合ros命令获取话题与消息信息。
编码实现运动控制节点。
启动 roscore、turtlesim_node 以及自定义的控制节点，查看运行结果。

1.话题与消息获取

准备: 先启动键盘控制乌龟运动案例。

启动乌龟运动显示节点：rosrun turtlesim turtlesim_node

启动键盘控制节点：rosrun turtlesim turtle_teleop_key

1.1话题获取

获取话题:/turtle1/cmd_vel

通过计算图查看话题，启动计算图:

rqt_graph
或者通过 rostopic 列出话题:

rostopic list

1.2消息获取

获取消息类型:geometry_msgs/Twist

rostopic type /turtle1/cmd_vel
获取消息格式:rosmsg info geometry_msgs/Twist
响应结果:

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)；

angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)。

2.实现发布节点

创建功能包需要依赖的功能包: roscpp rospy std_msgs geometry_msgs

代码test01_pub_twist.cpp

/*
    编写 ROS 节点，控制小乌龟画圆

    准备工作:
        1.获取topic(已知: /turtle1/cmd_vel)
        2.获取消息类型(已知: geometry_msgs/Twist)
        3.运行前，注意先启动 turtlesim_node 节点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建发布者对象
        4.循环发布运动控制消息
*/

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"my_control");
    ros::NodeHandle nh;
    // 3.创建发布者对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    //4-2.设置发送频率
    ros::Rate rate(10);
    // 4.循环发布运动控制消息
    //4-1.组织消息
    geometry_msgs::Twist twist;
    twist.linear.x = 1.0;
    twist.linear.y= 0.0;
    twist.linear.z = 0.0;

    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.5;
    //4-3.循环发送
    while(ros::ok())
    {
       pub.publish(twist);
       //休眠
       rate.sleep();
       //回头
       ros::spinOnce(); 
    }
    return 0;
}

系统自带话题控制乌龟以固定频率运动：

自定义话题控制乌龟以固定频率运动:

rosrun plumbing_test test01_pub_twist

项目二：话题订阅

需求描述: 已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

实现分析:

首先，需要启动乌龟显示以及运动控制节点并控制乌龟运动。
要通过ROS命令，来获取乌龟位姿发布的话题以及消息。
编写订阅节点，订阅并打印乌龟的位姿。

实现流程:

通过ros命令获取话题与消息信息。
编码实现位姿获取节点。
启动 roscore、turtlesim_node 、控制节点以及位姿订阅节点，控制乌龟运动并输出乌龟的位姿。

1.话题与消息获取

获取话题:/turtle1/pose

rostopic list

获取消息类型:turtlesim/Pose

rostopic type /turtle1/pose
获取消息格式:

rosmsg info turtlesim/Pose
响应结果:

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

theta：朝向

linear_velocity：线速度

angular_velocity：角速度

2.实现订阅节点

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

创建一个launch文件同时启动乌龟显示和控制运动功能start_turtle.launch

<!-- 启动乌龟GUI与键盘控制节点 -->
<launch>
    <!-- 乌龟GUI -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output= "screen" />
    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />

</launch>

在终端中先source ./devel/setup.bash，然后启动roslaunch plumbing_test start_turtle.launch

首先以命令的方式获取乌龟位姿：rostopic echo /turtle1/pose

然后编写代码test02_sub_pose.cpp

/*  
    订阅小乌龟的位姿: 时时获取小乌龟在窗体中的坐标并打印
    准备工作:
        1.获取话题名称 /turtle1/pose
        2.获取消息类型 turtlesim/Pose
        3.运行前启动 turtlesim_node 与 turtle_teleop_key 节点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅者对象
        5.回调函数处理订阅的数据
        6.spin
*/

#include "ros/ros.h"
#include "turtlesim/Pose.h"
void doPose(const turtlesim::Pose::ConstPtr &pose){
    ROS_INFO("乌龟的位姿坐标：坐标(%.2f,%.2f),朝向(%.2f),线速度：%.2f,角速度：%.2f",
    pose->x,pose->y,pose->theta,pose->linear_velocity,pose->angular_velocity);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"sub_pose");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
    // 5.回调函数处理订阅的数据
    ros::spin();
    return 0;
}

修改配置文件package.xml

<build_depend>turtlesim</build_depend>

<exec_depend>turtlesim</exec_depend>

修改配置文件CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  turtlesim
)

add_executable(test02_sub_pose src/test02_sub_pose.cpp)

add_dependencies(test02_sub_pose ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(test02_sub_pose
  ${catkin_LIBRARIES}
)

重新编译且source ./devel/setup.bash后，再rosrun plumbing_test test02_sub_pose，即可观测到位姿变换，有时候会这一步会出问题，重新启动vscode再编译即可。

项目三：服务调用

需求描述:编码实现向 turtlesim 发送请求，在乌龟显示节点的窗体指定位置生成一乌龟，这是一个服务请求操作。

实现分析:

首先，需要启动乌龟显示节点。
要通过ROS命令，来获取乌龟生成服务的服务名称以及服务消息类型。
编写服务请求节点，生成新的乌龟。

实现流程:

通过ros命令获取服务与服务消息信息。
编码实现服务请求节点。
启动 roscore、turtlesim_node 、乌龟生成节点，生成新的乌龟。

1.服务名称与服务消息获取

获取话题:/spawn

rosservice list

获取消息类型:turtlesim/Spawn

rosservice type /spawn

获取消息格式:

rossrv info turtlesim/Spawn

响应结果:

float32 x
float32 y
float32 theta
string name
---
string name

2.服务客户端实现

创建功能包需要依赖的功能包: roscpp rospy std_msgs turtlesim

/*
    生成一只小乌龟
    准备工作:
        1.服务话题 /spawn
        2.服务消息类型 turtlesim/Spawn
        3.运行前先启动 turtlesim_node 节点

    实现流程:
        1.包含头文件
          需要包含 turtlesim 包下资源，注意在 package.xml 配置
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 service 客户端
        5.等待服务启动
        6.发送请求
        7.处理响应

*/

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc,char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"service_call");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 service 客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 6.发送请求
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 4.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "turtle2";
    // 5.等待服务启动
    client.waitForExistence();
    bool flag = client.call(spawn);
    //flag接收响应状态，响应结果也会被设置金spawn对象
    // 7.处理响应结果
    if(flag)
    {
        ROS_INFO("乌龟生成成功，新乌龟叫:%s",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("请求失败！！！");
    }
    return 0;
}

1.roslaunch plumbing_test start_turtle.launch

其中URI指节点的资源路径

Args指请求服务时需要提交的资料

---上面是请求下面是响应

使用命令方式生成一只新乌龟

1.roslaunch plumbing_test start_turtle.launch

2.source ./devel/setup.bash

3.rosrun plumbing_test demo03_service_client

项目四：参数设置

需求描述: 修改turtlesim乌龟显示节点窗体的背景色，已知背景色是通过参数服务器的方式以 rgb 方式设置的。

实现分析:

首先，需要启动乌龟显示节点。
要通过ROS命令，来获取参数服务器中设置背景色的参数。
编写参数设置节点，修改参数服务器中的参数值。

实现流程:

通过ros命令获取参数。
编码实现参数设置节点。
启动 roscore、turtlesim_node 与参数设置节点，查看运行结果。

1.参数名获取

获取参数列表:rosparam list

响应结果:

/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r

2.参数修改

#include "ros/ros.h"

/*
      需求：修改参数服务器中turtlesim 背景色相关的参数
          1.初始化ROS节点
          2.不一定需要创建节点句柄(和后续API有关)
          3.修改参数
*/
int main(int argc, char *argv[])
{
    //1.初始化ROS节点
    ros::init(argc,argv,"change_bgColor");
    //2.不一定需要创建节点句柄(和后续API有关)
    //ros::NodeHandle nh("turtlesim");
    //nh.setParam("background_r",255);
    //nh.setParam("background_g",255);
    //nh.setParam("background_b",255);

    ros::NodeHandle nh;
    nh.setParam("/turtlesim/background_r",0);
    nh.setParam("/turtlesim/background_g",50);
    nh.setParam("/turtlesim/background_b",100);
    //3.修改参数
    //ros::param::set("/turtlesim/background_r",0);
    //ros::param::set("/turtlesim/background_g",0);
    //ros::param::set("/turtlesim/background_b",0);
    return 0;
}

1.roscore

2.rosrun plumbing_test test04_param

3.rosrun turtlesim turtlesim_node