CMakeList.txt 文件中的基本配置不再赘述

代码模板

c++ 模板

1
2
3
4
5
6
7
8
9
10
#include "ros/ros.h"

int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"");
ros::NodeHandle nh;

return 0;
}

python 模板

1
2
3
4
5
6
7
#! /usr/bin/env python

import rospy

if __name__ == "__main__":
rospy.init_node("")

ROS通信机制

先创建一个启动乌龟界面的launch文件

1
2
3
4
5
6
<launch>
<!-- 乌龟GUI -->
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output = "screen"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="turtle_key" output = "screen" />
</launch>

实操1 话题发布

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

实现分析:

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

实现流程:

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

话题与消息获取

先启动键盘控制乌龟运动案例

  1. 话题获取

    通过 rostopic 列出话题

    1
    rostopic list

    或者通过计算图查看话题,启动计算图

    1
    rqt_graph
    image-20250523203511067

    -> /turtle1/cmd_vel 传递运动指令

  2. 消息获取

    获取消息类型:

    1
    2
    rostopic type /turtle1/cmd_vel
    // rostopic info /turtle1/cmd_vel

    info 命令下也显示type

    type: geometry_msgs/Twist

  3. 获取消息格式

    1
    rosmsg info geometry_msgs/Twist

    输出:

    1
    2
    3
    4
    5
    6
    7
    8
    geometry_msgs/Vector3 linear
    float64 x
    float64 y
    float64 z
    geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z

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

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

    弧度:单位弧度定义为圆弧长度等于半径时的圆心角

    在ros中顺时针弧度为 - ,逆时针为 +,值的范围在[-π,π]

于是可以通过直接发布指令做到简单实现

1
rostopic pub -r 10 /turtle1/cmd_vel [TAB补齐]

实现发布节点

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

c++

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
/*
topic(已知: /turtle1/cmd_vel)
消息类型(已知: geometry_msgs/Twist)

实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建节点句柄
4.创建发布者对象
5.发布逻辑
6.spinOnce()
*/

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

int main(int argc, char *argv[])
{
ros::init(argc,argv,"circle_control");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);
geometry_msgs::Twist msg;

msg.linear.x = 1.0;
msg.linear.y = 1.0;
msg.linear.z = 0.0;
msg.angular.x = 0.0;
msg.angular.y = 0.0;
msg.angular.z = 1.0;

ros::Rate rate(10);
while(ros::ok())
{
pub.publish(msg);
rate.sleep();
ros::spinOnce();
}
return 0;

}

python

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

if __name__ == "__main__":

rospy.init_node("circle_control_p")
pub = rospy.Publisher("/turtle1/cmd_vel",Twist,queue_size=1000)
rate = rospy.Rate(10)
msg = Twist() // 注意这里要()

msg.linear.x = 1.0
msg.linear.y = 0.0
msg.linear.z = 0.0

msg.angular.x = 0.0
msg.angular.y = 0.0
msg.angular.z = 1.0

while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep

实操2 话题订阅

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

实现分析:

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

实现流程:

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

话题与消息获取

先启动键盘控制乌龟运动案例

  1. 话题获取

    1
    rostopic list

    -> /turtle1/pose 存储乌龟位置

  2. 获取消息类型

    1
    rostopic info  /turtle1/pose

    -> type: turtlesim/Pose

  3. 获取消息格式

    1
    rosmsg info turtlesim/Pose

    响应结果:

    1
    2
    3
    4
    5
    float32 x
    float32 y
    float32 theta
    float32 linear_velocity
    float32 angular_velocity

实现订阅节点

需要增添功能包turtlesim

在package.xml中增添

1
2
<build_depend>turtlesim</build_depend>
<exec_depend>sturtlesim</exec_depend>

在CMakeLsit.txt中增添

1
2
3
4
5
6
7
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
std_msgs
turtlesim //增添包
)

c++

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
/*
话题名称 /turtle1/pose
消息类型 turtlesim/Pose

实现流程:
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("乌龟的位姿信息: \n坐标(%.2f,%.2f)\n朝向(%.2f)\n线速度:%.2f\n角速度:%.2f",
pose->x,pose->y,pose->theta,pose->linear_velocity,pose->angular_velocity);
}


int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"sub_pose");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/turtle1/pose",1000,doPose);
ros::spin();
return 0;
}

python

1
2
3
4
5
6
7
8
9
10
11
12
13
#! /usr/bin/env python

import rospy
from turtlesim.msg import Pose

def doPose(pose):
rospy.loginfo("乌龟的位姿信息: \n坐标(%.2f,%.2f)\n朝向(%.2f)\n线速度:%.2f\n角速度:%.2f",
pose.x,pose.y,pose.theta,pose.linear_velocity,pose.angular_velocity)

if __name__ == "__main__":
rospy.init_node("sub_pose_p")
sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=1000)
rospy.spin()

代码中容易错误的地方:话题名是/turtle1/pose ,注意这个1

实操3 服务调用

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

实现分析:

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

实现流程:

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

服务名称与服务消息获取

  1. 获取话题

    1
    rosservice list

    -> /spawn

  2. 获取服务消息类型

    1
    rosservice info /spawn

    type: turtlesim/Spawn

    Args: x y theta name

  3. 获取服务消息格式

    1
    rossrv info turtlesim/Spawn

    响应结果:

    1
    2
    3
    4
    5
    6
    float32 x
    float32 y
    float32 theta
    string name
    ---
    string name

也可以简单实现增添乌龟

1
rosservice call /spawn [TAB补齐]

c++

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
/*
服务话题 /spawn
服务消息类型 turtlesim/Spawn

实现流程:
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,"");
ros::init(argc,argv,"service_call");
ros::NodeHandle nh;
ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn spawn;
spawn.request.x = 1.0;
spawn.request.y = 4.0;
spawn.request.theta = 1.57;
spawn.request.name = "turtlec";

// ros::service::waitForService("/spawn");
client.waitForExistence();
bool flag = client.call(spawn);
// 7.处理响应结果
if (flag)
{
ROS_INFO("新的乌龟生成,名字:%s",spawn.response.name.c_str());
} else {
ROS_INFO("乌龟生成失败!!!");
}
return 0;
}

python

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
#! /usr/bin/env python

import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse

if __name__ == "__main__":

rospy.init_node("service_call_p")
client = rospy.ServiceProxy("/spawn",Spawn)
request = SpawnRequest()
request.x = 4.0
request.y = 1.0
request.theta = 1.57
request.name = "turtlep"
client.wait_for_service()
try:
response = client.call(request)
rospy.loginfo("乌龟创建成功!,叫:%s",response.name)
except:
rospy.loginfo("服务调用失败")

可能会出现因为重复命名的异常

实操4 参数设置

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

实现分析:

  1. 首先,需要启动乌龟显示节点[注意这里不启动launch文件]
  2. 要通过ROS命令,来获取参数服务器中设置背景色的参数
  3. 编写参数设置节点,修改参数服务器中的参数值

实现流程:

  1. 通过ros命令获取参数
  2. 编码实现服参数设置节点
  3. 启动 roscore、turtlesim_node 与参数设置节点,查看运行结果

参数名获取

获取参数列表:

1
rosparam list

响应结果:

1
2
3
4
5
6
/turtle1/background_b
/turtle1/background_g
/turtle1/background_r
/turtlesim/background_b
/turtlesim/background_g
/turtlesim/background_r

方式1:命令行实现

1
2
3
rosparam set /turtlesim/background_r 自定义数值
rosparam set /turtlesim/background_g 自定义数值
rosparam set /turtlesim/background_b 自定义数值

修改相关参数后,重启 turtlesim_node 节点,背景色就会发生改变了

方式2:代码

c++

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#include "ros/ros.h"

int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"color");
ros::NodeHandle nh;
nh.setParam("/turtlesim/background_r",0);
nh.setParam("/turtlesim/background_g",0);
nh.setParam("/turtlesim/background_b",0);

// ros::NodeHandle nh("turtlesim"); //加上命名空间
// nh.setParam("background_r",0);
// nh.setParam("background_g",0);
// nh.setParam("background_b",0);
return 0;
}

python

1
2
3
4
5
6
7
8
9
#! /usr/bin/env python

import rospy

if __name__ == "__main__":
rospy.init_node("color_p")
rospy.set_param("/turtlesim/background_r",255)
rospy.set_param("/turtlesim/background_g",255)
rospy.set_param("/turtlesim/background_b",255)