chan's Bloggerrrrr

Rome was not built in a day.


  • 首页

  • 分类

  • 归档

xacro模型——阿克曼(Ackermann)四轮小车模型

发表于 2020-04-01 | 分类于 ROS机器人操作系统

Github链接:https://github.com/chanchanchan97/ROS

1.模型描述

本项目的目的是创建一个基于阿克曼转向机构(Ackermann steering)的自动驾驶小车模型,该模型在之前URDF模型的基础上修改了前轮转向轴的形状与位置,修改了前端摄像头的位置和尺寸,并且添加了激光雷达和树莓派的简易模型。

2.xacro模型与URDF模型的区别

2.1URDF模型存在的问题

●模型冗长,重复内容过多;
●参数修改麻烦,不便于二次开发;
●没有参数计算功能。

2.2xacro模型的特点

●精简模型代码
(1)创建宏定义
(2)文件包含
●提供可编程接口
(1)常量
(2)变量
(3)数学计算
(4)条件语句

3.xacro优化方法

3.1常量定义

1
<xacro:property name="M_PI" value="3.14159"/>

定义标签:xacro:property后面跟两个参数属性:
(1)name所定义的常量名;
(2)value常量名对应的常量值。

3.2常量使用

1
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>

定义标签:${ }在括号里使用常量,也可以进行数学运算(所有数学运算都会转换成浮点数进行,以保证运算精度)。

3.3宏定义

1
2
3
4
5
6
7
8
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>

定义标签:xacro:macro 后面跟两个参数属性:
(1)name宏定义的名字类似函数名;
(2)params类似函数参数,也可以是字符串。

3.4宏调用

1
<cylinder_inertial_matrix m="1" r="0.025" h="0.02"/>

定义标签:首先调用宏定义的名字,然后为参数赋值。

3.5文件包含

1
<xacro:include filename="$(find smartcar_description)/urdf/xacro/smartcar_sim.xacro"/>

定义标签:xacro:include $(find+功能包)=包的具体路径

4.创建xacro文件

在urdf文件夹下创建一个新的文件夹xacro,并且在xacro文件夹中创建一个.xacro文件。

5.xacro模型显示

5.1将xacro文件转化成URDF文件

在xacro文件夹中打开一个新的终端,输入以下指令:

1
$ rosrun xacro xacro.py smartcar.xacro > smartcar.urdf

此时xacro文件夹下会生成一个.urdf文件。

5.2直接调用xacro文件解析器

这种方法更为常见,在launch文件中加入如下代码,从而实现.xacro文件的转换。

1
2
3
<arg name="urdf_file" default="$(find xacro)/xacro --inorder '$(find smartcar_description)/urdf/xacro/smartcar.xacro'" />

<param name="robot_description" command="$(arg urdf_file)" />

5.3启动Rviz

打开一个新的终端,输入以下指令,启动Rviz,查看所创建的模型。

1
roslaunch smartcar_description smartcar_rviz.launch

URDF模型——阿克曼(Ackermann)四轮小车模型

发表于 2020-03-30 | 分类于 ROS机器人操作系统

Github链接:https://github.com/chanchanchan97/ROS

1.模型描述

本项目的目的是创建一个基于阿克曼转向机构(Ackermann steering)的简易四轮小车模型,该模型采用后轮双驱,而前轮在设计时选择独立转向,之后通过算法对两个前轮单独控制,模拟阿克曼转向模型。车体以简单的立方体来表示,并且在前端创建一个小立方体表示摄像头。

2.URDF文件

URDF全称为Unified Robot Description Format(统一机器人描述格式),是一种基于XML规范、用于描述机器人结构的格式。
从机构学角度讲,机器人通常被建模为由连杆(link)和关节(joint)组成的结构。连杆(link)是带有质量属性的刚体,而关节(joint)是连接、限制两个刚体相对运动的结构。通过关节将连杆依次连接起来,就构成了一个个运动链(也就是这里所定义的机器人模型)。URDF文件即描述了这样的一系列关节与连杆的相对关系、惯性属性、几何特点和碰撞模型,简单来说包含了机器人模型的运动学与动力学描述、机器人的几何表示、机器人的碰撞模型。

3.link元素

link元素描述了具有惯性,视觉特征和碰撞特性的刚体。以小车主体部分为例,创建如下的URDF文件内容。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
<link name="base_link"> 
<inertial>
<origin xyz="0 0 0.055"/>
<mass value="1.0" />
<inertia ixx="${1*(0.16*0.16+0.02*0.02)/12}" ixy="0.0" ixz="0.0" iyy="${1*(0.25*0.25+0.02*0.02)/12}" iyz="0.0" izz="${1*(0.16*0.16+0.25*0.25)/12}"/>
</inertial>
<visual>
<origin xyz="0 0 0.055" rpy="0 0 0" />
<geometry>
<box size="0.25 0.16 0.02"/>
</geometry>
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.055" rpy="0 0 0" />
<geometry>
<box size="0.25 0.16 0.02" />
</geometry>
</collision>
</link>

4.link属性详解

(1)name(必选)
link的名字,可以任意指定,在这里名字为base_link。
(2)inertial(可选)
◆origin(可选:如果未指定,则默认为identity)
这是惯性参考系相对于链路参考系的姿态。惯性参考系的原点需要位于重心处。惯性参考系的轴不需要与惯性的主轴对齐。
●xyz(可选:如果未指定,默认为零向量)
表示link刚体的重心位置分别在x轴,y轴,z轴方向上的偏移量。
●rpy(可选:如果未指定,则默认为identity)
以弧度表示固定轴的滚动,俯仰和偏航角度。
◆mass
链接的质量由此元素的value属性表示。
◆inertia
3x3转动惯量矩阵,以惯性框架表示。因为转动惯量矩阵是对称的,所以这里使用属性ixx,ixy,ixz,iyy,iyz,izz指定该矩阵的6个以上对角线元素。
(3)visual(可选)
link的可视属性。此元素指定对象的形状(例如:立方体(box),圆柱(cylinder)等)以用于可视化目的。
注意:同一链接(link)可以存在多个visual标记实例。
◆name
指定链接(link)几何的一部分的名称。不常用属性。
◆origin(可选:如果未指定,则默认为identity)
这是惯性参考系相对于链路参考系的姿态。惯性参考系的原点需要位于重心处。惯性参考系的轴不需要与惯性的主轴对齐。
●xyz(可选:如果未指定,默认为零向量)
表示link刚体的重心位置分别在x轴,y轴,z轴方向上的偏移量,单位米。
●rpy(可选:如果未指定,则默认为identity)
以弧度表示的固定轴滚动,俯仰和偏航角度。
◆geometry(必选)
●box(立方体)
size属性包含框的三个边长。立方体的原点在它的中心。
●cylinder(圆柱)
指定半径和长度。圆柱的原点位于其中心。
●sphere(球)
指定半径。球体的原点位于其中心。
●mesh
当想添加更为复杂的模型时,可以通过.stl等mesh文件导入模型。
◆material(可选)
视觉元素的材料。允许在顶层robot元素中指定link对象之外的材质元素。然后,您可以在链接元素中按名称引用材料。
●name
材料的名称。
●color(可选)
rgba由四个数字组成的材料颜色,分别表示红色/绿色/蓝色/alpha,每个数字的范围为[0,1]。alpha即色彩空间,也就是透明度/不透明度。
●texture(可选)
材质的纹理由文件名指定。
(4)collision(可选)
链接(link)的碰撞属性。请注意,这可能与链接的可视属性不同,例如,通常使用更简单的碰撞模型来减少计算时间。
注意:同一链接可以存在多个collision标记实例。
它们定义的几何的联合形成了链接的碰撞表示。该属性主要是在模拟时模型会与其他模型发生碰撞,不设置该属性的话会出现两个模型相互穿过的情况,设置该属性后在geometry设定的形状内两个模型会发生碰撞。
◆name(可选)
指定链接几何的一部分的名称。这对于能够引用链接几何的特定位是有用的。
◆origin(可选:如果未指定,则默认为identity)
碰撞元素的参考系相对于链接的参考系。
●xyz(可选:如果未指定,默认为零向量)
表示link刚体的重心位置分别在x轴,y轴,z轴方向上的偏移量,单位米。
●rpy(可选:如果未指定,则默认为identity)
以弧度表示的固定轴滚动roll,俯仰pitch和偏航yaw角度。
◆geometry
请参阅上述可视元素中的几何描述。

5.joint元素

joint元素描述了机器人关节的运动学和动力学,并且指定了关节的安全限制。以小车左后轮的关节为例,创建如下的URDF文件内容。

1
2
3
4
5
6
7
8
9
<joint name="left_back_wheel_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="left_back_wheel"/>
<origin rpy="0 ${M_PI/2} ${M_PI/2}" xyz="-0.08 0.08 0.025"/>
<dynamics damping="0.0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>

6.joint属性详解

(1)name(必选)
指定joint的名字(唯一的)
(2)type(必选)
指定joint的类型,有下列选项:
●revolute - 可以绕着一个轴旋转的铰链关节,有最大值和最小值限制。
●continuous - 连续型的铰链关节,可以绕一个轴旋转,没有最大值和最小值限制。
●prismatic - 滑动关节,可以沿着一个轴滑动,有最大值和最小值限制。
●fixed - 这不是一个实际的关节,因为它无法运动,所有的自由度都被锁定。这种类型的关节不需要指定轴、动力学特征、标度和最大值最小值限制。
●floating - 这是一个具有6个自由度的关节。
●planar - 此关节在一个平面内运动,垂线是运动轴。
◆origin(可选:如果未指定,则默认为identity)
从parent link到child link的变换,joint位于child link的原点,具体如图所示。
●xyz(可选:如果未指定,默认为零向量)
表示link刚体的重心位置分别在x轴,y轴,z轴方向上的偏移量,单位米。
●rpy(可选:如果未指定,则默认为identity)
以弧度表示的固定轴滚动,俯仰和偏航角度。
◆parent(必选)
parent link的名字是一个强制的属性,是这个link在机器人结构树中的名字。 可以理解为parent是主题,child是固定在主体上的配件。
◆child(必选)
child link的名字是一个强制的属性,是这个link在机器人结构树中的名字。
◆axis(可选: 默认为(1,0,0))
joint的axis轴在joint的坐标系中。这是revolute joint旋转的轴,prismatic joint移动的轴,是planar joint的标准平面。这个轴在joint坐标系中被指定。fixed和floating类型的joint不需要用到这个字段。
◆xyz(required)
代表轴向量的x,y,z分量,这应该是一个标准化的向量。
◆calibration(可选)
joint的参考点,用来矫正joint的绝对位置。
◆rising(可选)
当joint正向运动时,参考点会触发一个上升沿。
◆falling(可选)
当joint正向运动时,参考点会触发一个下降沿。
◆dynamics(可选)
该元素用来指定joint的物理性能。它的值被用来描述joint的建模性能,尤其是在仿真的时候。
◆damping(可选,默认为0)
joint的阻尼值(移动关节为,旋转关节为)
◆friction(可选,默认为0)
joint的摩擦力值(移动关节为,旋转关节为)
◆limit(只有type为revolute and prismatic时必选)
该元素包含以下属性:
◆lower(可选,默认为0)
指定joint运动范围下界的属性(revolute joint的单位为弧度,prismatic joint的单位为米),连续型的joint忽略该属性。
◆upper(可选,默认为0)
指定joint运动范围上界的属性(revolute joint的单位为弧度,prismatic joint的单位为米),连续型的joint忽略该属性。
◆effort(必选)
该属性指定了joint运行时的最大的力。(|applied effort|<|effort|)。
◆velocity(必选)
该属性指定了joint运行时的最大的速度。
◆mimic(可选)(New with ROS Groovy. See issue)
这个标签用于指定已定义的joint来模仿已存在的joint。这个joint的值可以用以下公式计算 value = multiplier * other_joint_value + offset. 有如下可选的属性:
●joint (required)
需要模仿的joint的名字。
◆multiplier(可选)
指定上述公式中的乘数因子。
◆offset(可选)
指定上述公式中的偏移项。默认值为0(revolute joint的单位为弧度,prismatic joint的单位为米)。
◆safety_controller(可选)
该元素包含下列属性:
◆soft_lower_limit(可选,默认为0)
该属性指定了joint安全控制边界的下界,是joint安全控制的起始限制点。这个值需要大于上述的limit中的lower值。
◆soft_upper_limit(可选,默认为0)
该属性指定了joint安全控制边界的上界,是joint安全控制的起始限制点。这个值需要小于上述的limit中的upper值。
◆k_position(可选,默认为0)
本属性用于说明位置和速度之间的关系。
◆k_velocity(必选)
本属性用于说明力和速度之间的关系。

7.Rviz可视化

Rviz是ROS官方提供的一款3D可视化工具,几乎我们需要用到的所有机器人相关数据都可以在Rviz中展现,但是不能模拟碰撞、重力等物理现象。

7.1创建launch文件

创建完URDF文件后,可以通过launch文件在Rviz中查看所创建的模型,launch文件具体内容如下。

1
2
3
4
5
6
7
8
9
10
<launch>     
<arg name="model" />
<arg name="gui" default="true" />
<param name="robot_description" textfile="$(find smartcar_description)/urdf/urdf/smartcar.urdf" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find smartcar_description)/config/smartcar.rviz" required="true"/>
</launch>

说明:
◆<arg name=”gui” default=”true” />的作用是使能joint_state_publisher节点的GUI界面,发布各个关节(joint)消息,验证各个关节(joint)功能。

◆启动joint_state_publisher节点和robot_state_publisher节点,joint_state_publisher发布/joint_states话题消息,robot_state_publisher则接收/joint_states话题消息,发布tf变换关系,并由Rviz显示出来。

◆args=”-d $(find smartcar_description)/config/smartcar.rviz”表示加载和保存的Rviz配置文件路径。

7.2启动Rviz

打开一个新的终端,输入以下指令,启动Rviz,查看所创建的模型。

1
$ roslaunch smartcar_description base_urdf_rviz.launch

说明:
◆若Rviz中没有正确显示模型,首先检查是否添加RobotModel,然后检查Fixed Frame是否为base_link。
◆若在左栏中添加TF,可以显示各个link之间的TF关系。

launch启动文件的使用

发表于 2020-03-27 | 分类于 ROS机器人操作系统

1.launch文件功能

之前运行ROS节点都需要先使用roscore命令启动ROS Master,并且单独rosrun命令启动各个ROS节点,而使用launch文件可以大大地简化这些步骤,只需要一个roslaunch命令,就可以同时启动ROS Master和多个节点程序。

2.launch文件语法

2.1<launch>标签

<launch>是launch文件的文件标签,是必须要有的。其中<launch>表示文件的起始,</launch>表示文件的结束。
例如:

1
2
3
4
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
</launch>

2.2<node>标签

<node>是节点标签,用来启动某个ROS节点,其中包含了如下几个主要的属性。
(1)pkg:表示节点所在的功能包名称,使用方式如pkg = “tf_lidar_task”;
(2)type:表示节点的可执行文件名称,使用方式如type = “lidar_broadcaster”;
(3)name:表示该节点在ROS系统中运行时的节点名,与节点编程时ros::init(argc, argv, “robot_tf_publisher”)中命名的节点名是同一概念,如果两者不同,launch文件中定义的name将覆盖程序中命名的节点名,可理解为重命名。因为ROS中节点名称是唯一的,name的使用是为了重复利用节点,比如发布广播t1和发布广播t2都使用发布广播节点,但运行时名称不同。使用方式如name = “broadcaster_t1”;
(4)output:用于控制某个节点是否将日志信息打印在终端界面上,默认输出到日志文档,使用方式如output = “screen”;
(5)respawn:表示在该节点突然停止时,是否会自动重启,默认为false,使用方式如respawn = “true”;
(6)required:表示该节点是否必须要启动,当该节点终止时,launch文件中的其他节点也被终止,使用方式如required = “true”;
(7)ns:表示namespace(命名空间),为节点内的相对名称添加命名空间前缀,避免命名冲突,使用方式如ns = “foo”;
(8)args:表示节点需要的输入参数,类似于之前ROS服务编程中实现整数加法时rosrun命令后面所跟着的参数,使用方式为args = “10 20”;

2.3<param>标签

<param>用于设置和修改ROS系统运行中的单个参数,存储在参数服务器中,类似于修改变量,例如可以设置turtlesim海龟界面背景的RGB值。使用方式如:

1
<param name="output_frame" value="odom"/>

其中,name是参数名,value是参数值。

2.4<rosparam>标签

<rosparam>用于加载参数文件中的多个参数,可以将参数文件中的所有参数都保存到参数服务器中。使用方式如:

1
<rosparam file="params.yaml" command="load" ns="local_costmap" />

其中,command属性可以设置为加载command=”load”,或设置为卸载command=”delete”。

2.5<arg>标签

<arg>与<param>、<rosparam>虽然都用于参数设置,但的区别在于它表示launch文件内部的局部变量,存储在launch文件中并且仅限于launch文件中使用。使用方式如:

1
<arg name="arg-name" value="arg-value" />

或
1
<arg name="arg-name" value="$(arg arg-name)" />

其中name是参数名,value是参数值。另外还可以为参数设置默认值。
1
<arg name="arg-name" default="1" />

2.6<remap>标签

<remap>表示重映射ROS计算图资源的命名,可近似理解为重命名,使用方式如:

1
<remap from="/turtlebot/cmd_vel"to"/cmd_vel"/>

其中from为原命名,to为映射之后的命名。

2.7<include>标签

<include>用于包含其他launch文件,类似于C语言中的头文件包含。使用方式如:

1
<include file="$(dirname)/turtle.launch" />

其中file是包含的其他launch文件路径。
更多资料可参考:http://wiki.ros.org/roslaunch/XML

3.launch文件使用

3.1创建launch文件

以TF变换的功能包为例,在tf_lidar_task工作空间中创建一个空文件夹,命名为launch。

在launch文件夹中创建一个空文件,命名为tf_lidar.launch。

3.2编译launch文件

在/ROS_ws文件夹路径下打开一个新的终端,输入以下代码进行编译。

1
$ catkin_make

3.3启动launch文件

在/ROS_ws路径下配置环境,再输入以下代码进行编译。

1
$ roslaunch tf_lidar_task tf_lidar.launch

3.4节点通信可视化

打开一个新的终端,输入以下代码。

1
$ rqt_graph

可以看到广播器已经开始发布TF变换关系到监听者节点,并且节点名也按照launch文件中重新命名。

TF坐标系变换的编程实现

发表于 2020-03-26 | 分类于 ROS机器人操作系统

1.创建功能包

在ROS工作空间ROS_ws的src文件夹目录下创建一个功能包,命名为tf_lidar_task,并编译完成。

2.节点编程

2.1案例说明

广播并监听机器人的坐标变换,已知激光雷达和机器人底盘的坐标关系,求解激光雷达数据在底盘坐标系下的坐标值。

2.2TF坐标系广播器编程

在功能包下面的src文件夹目录下创建一个空文件lidar_broadcaster.cpp,并打开所创建的文件,输入以下代码。

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
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

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

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

//设置TF广播频率为100Hz
ros::Rate r(100);

//创建tf::TransformBroadcaster类的对象
tf::TransformBroadcaster broadcaster;

while(n.ok())
{
//广播TF坐标系变换关系到ROS系统中
broadcaster.sendTransform( tf::StampedTransform( tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)), ros::Time::now(), "base_link", "base_laser"));

r.sleep();
}
}

说明:
◆头文件ros/ros.h包含了标准ROS类的声明,在每一个ROS程序中都需要包含它。
◆头文件tf/transform_broadcaster.h用来完成TF树的广播,之后会使用到tf::TransformBroadcaster类的实例。
◆main函数中一开始都是类似的,初始化ROS节点,创建节点句柄,从而启动ROS节点。
◆ros::Rate r(100);的作用是设置TF坐标系广播频率为100Hz。
◆tf::TransformBroadcaster broadcaster;的作用是创建一个tf::TransformBroadcaster类的对象,用来广播坐标系base_link -> base_laser的变换关系。
◆broadcaster.sendTransform(tf::StampedTransform())的作用是通过tf::TransformBroadcaster类调用sendTransform()函数,向系统中广播参考系之间的坐标变换关系,其中所广播的变换关系的数据类型是tf::StampedTransform,它包含了四个参数。
(1)第一个参数是存储坐标系之间变换关系的变量,此处tf::Transform是tf::StampedTransform的父类,它包含了两个参数。第一个参数表示坐标的旋转变换,通过tf::Quaternion四元数来存储旋转变换的参数,我们用到的两个参考系之间没有发生旋转变换,因此倾斜角、滚动角和偏航角都是0。第二个参数表示坐标的位移变换,这里我们将base_link作为父节点,base_laser作为子节点,从base_link节点到base_laser节点的变换关系为(x: 0.1m, y: 0.0m, z: 0.2m),将位移量填入到Vector3向量中。
(2)第二个参数是广播TF变换关系的时间戳,这里使用ros::Time::now()表示当前时间。
(3)第三个参数是传递的父节点坐标系的名称,即base_link。
(4)第四个参数是传递的子节点坐标系的名称,即base_laser。
tf::StampedTransform及tf::Transform类的定义:
http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1StampedTransform.html
http://docs.ros.org/jade/api/tf/html/c++/classtf_1_1Transform.html
◆r.sleep();的作用是为了延时,保证以100Hz的频率广播TF变换关系。
附相关资料:http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF

2.3TF坐标系监听者编程

在功能包下面的src文件夹目录下创建一个空文件lidar_listener.cpp,并打开所创建的文件,输入以下代码。

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
#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

//当接收到TF消息时,自动调用该回调函数
void transformPoint(const tf::TransformListener& listener)
{
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "base_laser";

laser_point.header.stamp = ros::Time();

laser_point.point.x = 1.0;
laser_point.point.y = 0.2;
laser_point.point.z = 0.0;

try
{
geometry_msgs::PointStamped base_point;
listener.transformPoint("base_link", laser_point, base_point);

ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
}
catch(tf::TransformException& ex)
{
ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
}
}

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

tf::TransformListener listener(ros::Duration(10));

ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

ros::spin();
}

说明:
◆头文件ros/ros.h包含了标准ROS类的声明,在每一个ROS程序中都需要包含它。
◆头文件geometry_msgs/PointStamped.h包含了坐标系和时间戳等信息,之后会使用geometry_msgs::PointStamped类创建一个虚拟点。
◆头文件tf/transform_listener.h用于创建TF变换的监听者,之后会使用到tf::TransformListener类的对象,该对象会自动订阅ROS中的TF消息,并且管理所有的变换关系数据。
◆transformPoint(const tf::TransformListener& listener)该回调函数每当接收到TF消息时,都会被自动调用。在回调函数中,我们需要完成数据从坐标系base_laser到base_link的坐标变换。
◆
1
2
3
4
5
6
7
8
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "base_laser";

laser_point.header.stamp = ros::Time();

laser_point.point.x = 1.0;
laser_point.point.y = 0.2;
laser_point.point.z = 0.0;

这里创建了一个geometry_msgs::PointStamped类型的虚拟点,该类型包含标准的header和point两种消息结构,因此可以在消息中添加发布数据的时间戳、参考系的ID以及该点的坐标。其中ros::Time()表示使用缓冲中最新的TF数据,这里不使用ros::Time::now()是因为每个监听器都有一个缓冲区,存储来自不同TF广播的所有坐标变换。当广播器发送TF变换时,数据进入缓冲区需要一段时间(通常是几毫秒)。因此如果在当前时刻就请求TF变换数据,缓冲区中还提取不到当前发布的变换,您应该等待一段时间,直到数据到达。
PointStamped的消息定义:http://docs.ros.org/api/geometry_msgs/html/msg/PointStamped.html
◆
1
2
3
4
5
6
7
try
{
geometry_msgs::PointStamped base_point;
listener.transformPoint("base_link", laser_point, base_point);

ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", laser_point.point.x, laser_point.point.y, laser_point.point.z, base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
}

这里使用tf::TransformListener类的对象中的transformPoint()函数,将虚拟点的坐标数据从base_laser参考系转换到base_link参考系下,该函数包含三个参数。
(1)第一个参数是需要转换到的参考系ID,即base_link。
(2)第二个参数是需要转换的原始数据,即上面创建的虚拟点的信息。
(3)第三个参数是存储TF变换完成后的数据的变量。
该函数执行完毕后,base_point中就包含了变换完成后的点坐标了,最后通过ROS_INFO()将坐标信息打印在ROS终端界面。
◆
1
2
3
4
catch(tf::TransformException& ex)
{
ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
}

如果由于某种原因导致base_lase -> base_link的转换不可用(tf_broadcaster未运行等),则在执行transformPoint()时就会出现错误。为了确保能够正常处理,我们将捕获异常并为用户打印错误信息。ex.what()是tf::TransformException类的一个方法,它返回一个指向与异常相关的内容的C字符串的指针。此处必须用try-catch语句,否则会报错。
◆main函数中一开始都是类似的,初始化ROS节点,创建节点句柄,从而启动ROS节点。
◆tf::TransformListener listener(ros::Duration(10))的作用是通过tf::TransformListener创建一个TF变换的监听者,形参中的ros::Duration()函数表示所存储的TF变换数据的时间长度,TF功能包最大可以存储10s的数据,10s之后就会将之前的消息丢弃掉,这里将数据存储的时间长度设置为10s。
tf::TransformListener类的定义:
http://docs.ros.org/diamondback/api/tf/html/c++/classtf_1_1TransformListener.html#a7e41bd5e244f92709e22bffa2860605d
◆ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); 此处创建了一个timer对象,通过ros::Duration()函数设置回调函数transformPoint()的执行频率为1Hz,通过boost::bind()函数将回调函数transformPoint()的参数与listener对象绑定,而当某些函数对象或值参数很大,拷贝代价很高,或者无法拷贝时,boost::bind()的使用就会受到限制,因此使用boost::ref()引用传递对象。
◆ros::spin();的作用是让程序进入自循环的挂起状态,从而让程序以最好的效率监听TF变换并调用回调函数。

3.配置与编译

3.1在CMaKeLists.txt中添加编译选项

将如下位置中CATLIN_DEPENDS前面的“#”去掉,使能相关的依赖项。

在如下位置进行配置,add_executable(lidar_broadcaster src/lidar_broadcaster.cpp)的作用是将src文件夹下的lidar_broadcaster.cpp文件编译成名为lidar_broadcaster的可执行文件。target_link_libraries(lidar_broadcaster ${catkin_LIBRARIES})的作用是将lidar_broadcaster可执行文件与ROS相关的库链接。

1
2
3
4
5
add_executable(lidar_broadcaster src/lidar_broadcaster.cpp)
add_executable(lidar_listener src/lidar_listener.cpp)

target_link_libraries(lidar_broadcaster ${catkin_LIBRARIES})
target_link_libraries(lidar_listener ${catkin_LIBRARIES})

3.2编译文件

在/ROS_ws文件夹路径下打开一个新的终端,输入以下代码进行编译。

1
$ catkin_make


编译完成后,输入以下代码运行主节点。
1
$ roscore


打开一个新的终端,配置环境变量后,输入以下代码运行TF广播器。
1
$ rosrun tf_lidar_task lidar_listener


打开一个新的终端,配置环境变量后,输入以下代码运行TF监听者。
1
$ rosrun tf_lidar_task lidar_broadcaster


若想停止运行,关闭终端,使用快捷键Ctrl+c即可。

4.话题可视化

在src文件夹路径下打开一个新的终端,输入以下代码。
◆

1
$ rosrun tf view_frames

由此可以生成一个名为frames的pdf文件,直观地看到TF变换时的坐标系关系。

◆
1
$ rosrun rqt_tf_tree rqt_tf_tree

该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到,例如何时断开何时连接,捕捉到这些然后通过rqt插件显示出来。

◆
1
$ rostopic echo /tf

该命令的作用是以TransformStamped消息类型的数组显示所有父子frame的位姿转换关系。

◆
1
$ rosrun tf tf_echo base_laser base_link

该命令的作用是查看任意两个frame之间的变换关系,终端将会持续的显示当前源坐标系和目标坐标系的位姿变换关系。

ROS动作服务器与客户端的内部交互(进阶)

发表于 2020-03-24 | 分类于 ROS机器人操作系统

原文链接:http://wiki.ros.org/actionlib/DetailedDescription

1.动作服务器

goal由action客户端发出,一旦action服务器接收到goal,就会创建一个状态机来跟踪goal的状态,如下图。

1.1服务器状态

◆Pending - 该目标尚未由动作服务器处理;
◆Active - 该目标当前正在由操作服务器处理;
◆Recalling - 目标尚未处理,并且已从动作客户端收到取消请求,但是动作服务器尚未确认目标已取消;
◆Preempting - 正在处理目标,并且已从操作客户端收到取消请求,但是操作服务器尚未确认目标已取消;
◆Rejected - 该目标已被动作服务器拒绝,未经处理,也没有动作客户端请求取消;
◆Succeeded - 动作服务器成功完成目标;
◆Aborted - 该目标已由动作服务器中止,而没有来自动作客户端的外部请求取消;
◆Recalled - 在动作服务器开始处理目标之前,该目标已被另一个目标或取消请求而取消;
◆Preempted - 目标的处理被另一个目标,或已发送到动作服务器的取消请求取消;

1.2状态转换命令

◆setAccepted - 检查目标后,决定开始处理它;
◆setRejected - 检查目标之后,决定不对其进行处理,因为它是无效的请求(超出范围,资源不可用,无效等);
◆setSucceeded – 通知目标已成功处理;
◆setAborted - 通知该目标在处理过程中遇到错误,必须中止;
◆setCanceled - 通知由于取消请求,而不再处理目标;
◆CancelRequest - 客户端通知动作服务器希望服务器停止处理目标。

2.动作客户端

在actionlib中,我们将服务器状态机视为主状态机,然后将客户端状态机视为试图跟踪服务器状态的辅助状态机,如下图。

2.1服务器触发转换

◆Reported [State]:由于客户端尝试跟踪服务器的状态,因此大多数状态的转换都是由服务器向动作客户端报告其状态来触发的;
◆Receive Result Message:在这种情况下,服务器将result发送给客户端。接收到result预示着跟踪目标的结束。

2.2客户端触发转换

◆Cancel Goal:请求服务器停止处理此目标。

2.3“跳过”状态

◆给定我们基于ROS的传输层,客户端可能无法从服务器接收所有状态更新。因此,我们必须允许客户端状态机“跳过”服务器触发的状态。
示例:如果客户端处于[WAITING FOR GOAL ACK]中,并且从服务器接收到[PREEMPTED]状态更新,则客户端状态可以跳过[ACTIVE]并直接转换为[WAITING FOR RESULT]。
◆由于多个动作客户端可以连接到单个动作服务器,因此第二个客户端可以取消第一个客户端发送的目标。因此,如果从服务器接收到[RECALLING]状态,则客户端从[PENDING]转换为[RECALLING]是有效的。

3.动作接口及传输层

Action客户端和服务器使用预定义的动作协议(action protocol)相互进行通信。 此动作协议依靠指定的ROS命名空间中的ROS话题来传输消息。

ROS消息:
◆goal – 用于发送新的目标(goal)给服务器。
◆cancel – 用于发送取消请求给服务器。
◆status – 用于将系统中每个目标(goal)的当前状态通知给客户端。
◆feedback - 用于定期向客户端发送目标(goal)的辅助信息。
◆result - 用于在目标(goal)完成后向客户发送一次辅助信息。

3.1数据关联与Goal ID

Goal ID是action message中一个字符串字段,这为动作服务器和客户端提供了一种可靠的方式,可以将通过ROS传输的消息与正在处理的特定goal相关联。 目标ID通常由节点名称,计数器和时间戳组成。
注意:goal ID的格式仍然不稳定,因此用户切勿解析goal ID或依赖其格式。

3.2消息

3.2.1goal topic: Sending Goals

Goal topic使用自动生成的ActionGoal消息(例如:actionlib/TestActionGoal),用于将新目标发送到动作服务器。ActionGoal消息将目标消息打包,并将其与目标ID捆绑在一起。
发送目标时,动作客户端通常会生成唯一的goal ID和时间戳,但是客户端也可能会将它们留空,此时动作服务器将会填充它们。
◆Empty stamp:一旦动作服务器接收到,时间戳将设置为now()。
◆Empty id:一旦动作服务器接收到,ID自动随机创建。不过这个ID由于动作客户端无法知道其对应的goal,所以意义不大。

3.2.2cancel topic: Cancelling Goals

Cancel topic使用actionlib_msgs/GoalID消息,并允许动作客户端将取消请求发送给动作服务器。每个取消请求消息都带有一个时间戳和goal ID,并且如何填充这些消息字段将会影响到要取消的目标,下图是取消请求的处理策略。

3.2.3status topic: Server goal state updates

Status topic使用actionlib_msgs / GoalStatusArray,并提供给动作客户端有关服务器当前正在追踪的每个目标的状态信息。这是由服务器以某个固定速率(通常为10Hz)发送的,并且还可以在任何服务器目标状态转换时异步发送。
动作服务器会追踪目标,直到达到终端状态为止。 但是为了提高通信的健壮性,服务器在达到终端状态后,会再次发布此目标的状态几秒钟。

3.2.4feedback topic: Asynchronous goal information

Feedback topic使用自动生成的ActionFeedback消息(例如:todo),并为服务器提供了一种在处理目标期间向动作客户端定期发布更新的方法。由于ActionFeedback具有goal ID,因此动作客户端可以决定它是否使用还是丢弃所收到的反馈消息,因此发送反馈完全是可选的。

3.2.5 result topic: Goal information upon completion

Result topic使用自动生成的ActionResult消息(例如:actionlib/TestActionResult),并为服务器实现者提供了一种在目标完成后向操作客户端发送信息的方法。由于ActionResult具有goal ID,因此动作客户端可以决定它是否使用还是丢弃result消息。尽管reslt可能是空消息,但它是action接口的必需部分,并且必须始终在目标完成后发送,因此必须在转换到终端状态(Rejected, Recalled, Preempted, Aborted, Succeeded)时发送结果。

4机制

4.1 Simple Action Client

通常,高层次的应用工程师和管理人员仅仅关心的是,目标是否正在处理或目标是否完成,他们很少关心所有中间状态。Simple action client将原始的客户端状态机分为三个状态:Pending,Active和Done。

4.1.1客户端状态歧义

仅客户端状态不足以确定Simple client状态,但是,这可以通过查看客户端状态转换轻松解决。如果客户端状态转换未在任何Simple client状态之间交叉,则不会更新Simple client状态。
示例:如果客户端从RECALLING转换为WAITING FOR RESULT,则简单客户端状态仍将保持PENDING状态。

4.1.2多目标策略

为简单起见,Simple Action Client一次仅跟踪一个目标。当用户通过Simple client发送目标时,它会禁用与先前目标相关的所有回调函数,并且还会停止跟踪其状态,但是它不会取消先前的目标。

4.1.3线程模型(C++)

在构造Simple Action Client时,用户可决定是否扩展额外的线程。
◆无多余的线程(推荐)
动作客户端中的所有订阅者都向全局回调队列注册。从ros::spin()内部调用用户的action回调,因此阻止用户的action回调将阻止为全局回调队列提供服务。
◆扩展线程
在动作客户端中的所有订阅者都注册有一个回调队列,该队列与全局回调队列分开,并且由扩展的线程进行服务。
从所扩展的线程中调用用户的action回调,尽管阻止action回调不会阻止其他ROS消息得到服务,但这仍然不是一个好主意,因为该服务的status,feedback和result消息无法得到服务。
扩展额外线程的好处是,用户可以避免在其应用程序中调用ros::spin()。

4.2Simple Action Server

很多动作服务器都遵循类似的模式,一次只能激活一个目标,而每个新目标都将优先于前一个目标。Simple Action Server是基于动作服务器的包装,旨在为执行目标而实施此简化策略。

在接收到从动作客户端发出的新目标后,Simple Action Server将该目标挂起。如果一个目标已经占用了挂起位置,则Simple Action Server将这个已占用的目标设置为取消,并将其替换为通过网络传入的目标。

一旦Simple Action Server接收到一个新目标并将其挂起,就会向Simple Action Server的用户通知有一个新目标可用。如以下目标通知一节所述,此通知以两种方式中的其中一种方式发生。在接收到通知后,用户可以接收使被挂起的目标移动到当前目标的目标,并允许用户修改与新接收的目标相关联的状态机。

4.2.1目标通知

用户可以通过两种方式去接收Simple Action Server已接收到新目标的通知。
◆回调通知(Callback Notification):用户在构造中向Simple Action Server注册了一个回调,当新目标被Simple Action Server挂起时,将调用该回调函数。用户可以在回调中接收新目标,或者在准备就绪时通知另一个线程接收目标。
◆轮询通知(Polling Notification):用户明确询问Simple Action Server是否有新的目标可用。Simple Action Server根据从上一次进行新目标查询以来是否有新的目标被挂起,从而对此查询进行回应。

4.2.2线程模型(C++)

在构造Simple Action Server时,用户决定是否增加一个额外的线程以允许在目标回调函数中执行长时间运行的动作。
◆无多余的线程(推荐)
在为接收新目标的回调中进行的任何操作都不应长期运行,用户可以通知另一个线程进行工作,但不应阻塞。用户也可以使用轮询来检查新目标的有效性,并完全可以避免回调。
◆扩展线程
创建一个独立的线程以允许用户当新目标有效时,能够在接收到的回调函数中执行长时间的运行或被阻止的动作。在此回调中,用户还可以轮询Simple Action Server以检查是否有新目标。
Simple Action Server为用户扩展线程的好处是,用户不必处理用于管理另一个线程的开销。但是对于用户而言,重要的是要意识到此线程的存在,以便它们遵循诸如锁定之类的标准线程安全规定。

ROS动作通信的编程实现

发表于 2020-03-23 | 分类于 ROS机器人操作系统

1.创建功能包

在ROS工作空间ROS_ws的src文件夹目录下创建一个功能包,命名为action_task。

2.节点编程与动作消息定义

2.1案例说明

客户端发送一个运动目标,模拟机器人运动到目标位置的过程,包含服务端和客户端的代码实现,要求带有实时位置反馈。

2.2动作消息的定义

在功能包目录下创建一个新的文件夹,命名为action,并在此文件夹中创建一个空文件Motion.action。

在Motion.action文件中输入以下代码,定义动作消息内容。

1
2
3
4
5
6
7
8
9
10
//定义机器人运动终点坐标endx,endy
uint32 endx
uint32 endy
---
//定义机器人动作完成标志位
uint32 Flag_Finished
---
//定义机器人当前位置坐标coordinate_x,coordinate_y
uint32 coordinate_x
uint32 coordinate_y

说明:
动作(Action)通信接口提供了五种消息定义,分别为goal、cancel、status、feedback和result,而.action文件用来定义其中三种消息,按顺序分别为goal、result和feedback,与.srv文件中的服务消息定义方式一样,使用“—-”作为分隔符。
Motion.action文件经过编译后生成MotionAction.h、MotionActionFeedback.h和MotionGoal.h等多个头文件,文件位置在ROS_ws工作空间下的devel/include/action_task文件夹内,如下图。

附相关资料:http://wiki.ros.org/actionlib#Tutorials

2.3创建.cpp文件

在功能包下面的action文件夹目录下创建一个空文件robotclient.cpp。

2.4动作客户端编程

打开上面所创建的文件robotclient.cpp,输入以下代码。

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
44
45
46
47
48
49
50
51
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include "action_task/MotionAction.h"

typedef actionlib::SimpleActionClient<action_task::MotionAction> Client;

//当动作完成后,调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
const action_task::MotionResultConstPtr& result)
{
ROS_INFO("The robot has arrived at the destination.");
ros::shutdown();
}

//当动作被激活,调用该函数一次
void activeCb()
{
ROS_INFO("Goal just went active");
}

//接收到feedback后,调用该回调函数
void feedbackCb(const action_task::MotionFeedbackConstPtr& feedback)
{
ROS_INFO(" The place of robot : (%d , %d) ", feedback->coordinate_x, feedback->coordinate_y);
}

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

//创建一个action客户端
Client client("robot_motion", true);

//等待action服务器响应
ROS_INFO("Waiting for action server to start.");
client.waitForServer();
ROS_INFO("Action server started, sending goal.");

//创建一个action目标
action_task::MotionGoal goal;
goal.endx = 5;
goal.endy = 4;

//发送action目标给服务器,并设置回调函数
client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

ros::spin();

return 0;
}

说明:
◆头文件actionlib/client/simple_action_client.h用于实现简单的动作客户端。其中actionlib软件包提供了创建执行长期目标的服务器,以及一个客户端接口,以便将请求发送到服务器。
◆头文件action_task/MotionAction.h是由Motion.action文件编译生成,定义了动作消息内容,该文件存放在ROS_ws工作空间下的devel/include/action_task文件夹中。
◆typedef actionlib::SimpleActionClient Client;的作用是使用Client声明定义action_task::MotionAction类型的actionlib::SimpleActionClient类。
◆doneCb(const actionlib::SimpleClientGoalState& state, const action_task::MotionResultConstPtr& result)该函数在服务器完成任务后,通知客户端调用一次,调用该回调函数后ROS系统在终端界面输出字符串”The robot has arrived at the destination.”表明动作完成,同时ros::shutdown()关闭客户端节点,终止所有开放的订阅,发布,服务及调用。
◆void activeCb()该函数是在动作服务器被激活后,通知客户端开始执行任务的回调函数,调用该回调函数后ROS系统在终端界面输出字符串”Goal just went active”表明action被激活。
◆void feedbackCb(const action_task::MotionFeedbackConstPtr& feedback)该函数是在动作服务器执行任务过程中,通知客户端机器人当前坐标的回调函数。const action_task::MotionFeedbackConstPtr& feedback这里存放的是由主调函数放进来的实参变量feedback的地址,通过引用传递给回调函数。调用该回调函数后ROS系统在终端界面输出当前机器人位置的横纵坐标。
◆main函数中首先使用ros::init()初始化ROS节点,将该节点命名为robot_client。
◆Client client(“robot_motion”, true);的作用是创建一个action_task::MotionAction类型的action客户端。Client是之前已经使用typedef重新声明定义的actionlib::SimpleActionClient,client是一个action_task::MotionAction消息类型的对象,第一个参数表示action客户端与服务器之间通信的消息名称,第二个参数true表示action客户端以单线程运行。
◆client.waitForServer()函数的作用是等待客户端连接到动作服务器,否则客户端将停在这里一直处于等待,不过也可以使用ros::duration()函数作为参数设置等待时间。
◆action_task::MotionGoal goal是创建一个action_task::MotionGoal类的对象goal,并为goal成员endx、endy即终点坐标赋值。
◆client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);的作用是发送一个目标值到action服务器,并设置多个回调函数检测goal的执行情况(action服务器和客户端分别在接收和发送goal之后会通过状态机追踪goal的状态,并采取相应的处理)。
◆ros::spin();的作用是让程序进入自循环的挂起状态,从而让程序以最好的效率接收客户端的请求并调用回调函数。

2.5action服务器编程

在src文件夹下再创建一个空文件robotserver.cpp,输入以下代码。

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
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "action_task/MotionAction.h"

#define wid 5
#define hig 5

typedef actionlib::SimpleActionServer<action_task::MotionAction> Server;

//定义结构体
struct note
{
int x;//横坐标
int y;//纵坐标
int f;//父节点在队列中的编号
int s;//步数
};

//接收到action的goal之后,调用该回调函数一次
void execute(const action_task::MotionGoalConstPtr& goal, Server* as)
{
struct note que[40];//定义一个note结构体的队列

//定义地图大小及形式,1为障碍物,0为正常道路
int map[6][6]={{0, 0, 1, 0, 0, 1},
{1, 0, 1, 0, 0, 0},
{1, 0 ,0, 1, 0, 1},
{0, 1, 0, 0, 0, 0},
{0, 0, 0, 1, 1, 0},
{1, 0, 0, 1, 0, 0}};

//记录哪些点已经在队列中,防止一个点被重复扩展
int book[6][6]={0};

//定义一个表示走的方向的数组
int next[4][2]={{0,1}, //向右
{1,0}, //向下
{0,-1},//向左
{-1,0}};//向上

int head, tail;
int j, k, l;
int startx = 0, starty = 0;
int p,q,tx,ty,flag;

ros::Rate r(1); //设置ROS系统延时频率

action_task::MotionFeedback feedback; //创建一个feedback对象

//初始化队列
head=0;//head point to the point needed to expand
tail=0;//tail point to the point expanded

//定义起点坐标
que[tail].x=startx;
que[tail].y=starty;
que[tail].f=0;
que[tail].s=0;
tail++;
book[startx][starty]=1;

flag=0;//用来标记是否到达目标点,0表示还没有到达,1表示已到达

//当队列不为空时循环
while(head<tail)
{
//枚举四个方向
for(k=0;k<=3;k++)
{
//计算下一个点的坐标
tx=que[head].x+next[k][0];
ty=que[head].y+next[k][1];

//判断是否越界
if(tx<0 || tx>wid || ty<0 || ty>hig)
continue;
//判断是否为障碍物或已经在路径中
if(map[tx][ty]==0 && book[tx][ty]==0)
{
//标记这个点已经被走过
book[tx][ty]=1;
//插入新的坐标到队列中
que[tail].x=tx;
que[tail].y=ty;
que[tail].f=head;
que[tail].s=que[head].s+1;
tail++;
}

feedback.coordinate_x = tx;
feedback.coordinate_y = ty;
as->publishFeedback(feedback);//按照1Hz的频率发送机器人当前坐标位置
r.sleep();//延时至1s

if(tx==goal->endx && ty==goal->endy)
{
flag=1;//到达目标点后标志位flag置1
break;
}
}

if(flag==1)
break;
head++;
}

//表示已经发送成功
as->setSucceeded();
}

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

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

//创建一个action服务器
Server server(n, "robot_motion", boost::bind(&execute, _1, &server), false);

//启动action服务器
server.start();

ros::spin();

return 0;
}

说明:
◆头文件ros/ros.h包含了标准ROS类的声明,在每一个ROS程序中都需要包含它。
◆头文件actionlib/server/simple_action_server.h用于实现简单的动作服务端。
◆头文件action_task/MotionAction.h 是由Motion.action文件编译生成,定义了动作消息内容,该文件存放在ROS_ws工作空间下的devel/include/action_task文件夹中。
◆typedef actionlib::SimpleActionServer Server;的作用是使用Server声明定义action_task::MotionAction类型的actionlib::SimpleActionServer类。
◆void execute(const action_task::MotionGoalConstPtr& goal, Server as)当action服务器接收到客户端的goal时调用该回调函数。其中第一个参数是通过引用向回调函数传递的指向goal的指针,goal的消息类型为action_task::MotionGoal,第二个参数是actionlib::SimpleActionServer类的指针as,消息类型为action_task::MotionAction。
◆在execute()回调函数中通过建立一个6×6的数组模拟机器人运行的地图,根据action客户端发来的goal目标点坐标,使用广度优先搜索算法寻找可运行的路径,服务器按照1Hz频率发布feedback。
◆ros::Rate r(1);的作用是设置feedback发布频率为1Hz。
◆action_task::MotionFeedback feedback;的作用是创建一个action_task::MotionFeedback类的对象feedback。
◆as->publishFeedback(feedback);调用指针as指向的actionlib::SimpleActionServer类所包含的publishFeedback()函数,publishFeedback (const Feedback &feedback)函数用于发布feedback。
◆r.sleep();该函数并不是必要的,这里的作用是为了延时,让服务器保证以1Hz的频率发布feedback。
◆as->setSucceeded();调用指针as指向的actionlib::SimpleActionServer类所包含的setSucceeded ()函数,setSucceeded (const Result &result=Result(), const std::string &text=std::string(“”))函数中第一个参数表示动作执行任务的结果result,默认为0,第二个参数是有关状态更改的文本消息,两个参数均可省略。
◆main函数中一开始都是类似的,初始化ROS节点,创建节点句柄,从而启动ROS节点。
◆Server server(n, “robot_motion”, boost::bind(&execute, _1, &server), false);的作用是创建一个action服务器,其中第一个参数表示所创建节点句柄名;第二个参数表示服务器与客户端之间通信的消息名;第三个参数表示所要调用的回调函数,当接收到新的goal后,就会在单独的线程中调用该回调函数。boost::bind()函数用来向一个函数(或函数对象)绑定某些参数,返回值是一个函数对象,其中第一个参数是需要绑定的原始函数的地址,此处为&execute,第二个参数是需要绑定到原始函数excute()的第一个参数值,而此处_1表示execute(const action_task::MotionGoalConstPtr& goal, Server
as)函数的第一个参数不需要绑定,因此使用_1占位,第三个参数是需要绑定到原始函数excute()的第二个参数值,此处为&server表示将server对象的地址绑定到execute()函数中的Server* as指针。server(n, “robot_motion”, boost::bind(&execute, _1, &server), false)中的第四个参数是一个bool值,它的作用是通知action服务器是否在被创建后立即开始发布,此参数应当始终设置为false,并且在服务器被创建后调用start()。
◆server.start();的作用是启动action服务器,使用时需保证创建action服务器时第四个参数auto_start设置为false。
◆ros::spin();的作用是让程序进入自循环的挂起状态,从而让程序以最好的效率接收客户端的请求并调用回调函数。
附相关资料:
1.http://wiki.ros.org/actionlib_tutorials/Tutorials/SimpleActionServer%28ExecuteCallbackMethod%29
2.https://docs.ros.org/diamondback/api/actionlib/html/classactionlib_1_1SimpleActionServer.html
3.http://docs.ros.org/melodic/api/actionlib/html/classactionlib_1_1ServerGoalHandle.html#ac297923512ac62c9f10a801571b29738

3.配置与编译

3.1在CMaKeLists.txt中添加编译选项

打开功能包中的CMaKeLists.txt文件,在如下位置的find_package中添加功能包actionlib和actionlib_msgs,以便于(节点)调用它们生成消息。

在如下位置添加相关的.action文件,确保了CMake在重新配置时知道这些新添加的.action文件,同时添加.action文件在生成消息时的所有依赖项(功能包)。

1
2
3
add_action_files(DIRECTORY action FILES Motion.action)

generate_messages(DEPENDENCIES actionlib_msgs std_msgs)


将如下位置中CATLIN_DEPENDS前面的“#”去掉,使能相关的依赖项。

在如下位置进行配置,add_executable(robotclient src/robotclient.cpp)的作用是将src文件夹下的robotclient.cpp文件编译成名为robotclient的可执行文件。target_link_libraries(robotclient ${catkin_LIBRARIES})的作用是将robotclient可执行文件与ROS相关的库链接。add_dependencies(robotclient ${${PROJECT_NAME}_EXPORTED_TARGETS})的作用是创建一个显式的依赖,以便相关文件能以正确的顺序编译。
1
2
3
4
5
6
7
add_executable(robotclient src/robotclient.cpp)
target_link_libraries(robotclient ${catkin_LIBRARIES})
add_dependencies(robotclient ${${PROJECT_NAME}_EXPORTED_TARGETS})

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

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

打开功能包中的package.xml文件,在如下位置添加功能包依赖。<build_depend>actionlib</build_depend>表示在编译时会依赖actionlib功能包。<exec_depend>actionlib</exec_depend>表示在运行时会依赖actionlib功能包。

1
2
3
4
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>

3.3编译文件

在/ROS_ws文件夹路径下打开一个新的终端,输入以下代码进行编译。

1
$ catkin_make


编译完成后,输入以下代码运行主节点。
1
$ roscore


打开一个新的终端,配置环境变量后,输入以下代码运行客户端。
1
$ rosrun action_task robotclient


打开一个新的终端,配置环境变量后,输入以下代码运行服务器。
1
$ rosrun action_task robotserver


若想停止运行,关闭终端,使用快捷键Ctrl+c即可。

4.话题可视化

打开一个新的终端,在action服务器及客户端运行时输入以下代码。

1
$ rqt_graph

由此可以得到如下的基于Qt的GUI界面,直观地看到动作通信的节点和消息。

ROS服务通信的编程实现

发表于 2020-01-12 | 分类于 ROS机器人操作系统

1.创建功能包

在ROS工作空间ROS_ws的src文件夹目录下创建一个功能包,命名为communication_pkg,并编译完成。

2.节点编程与服务数据定义

2.1案例说明

定义一个客户端Client,通过自定义的服务请求Request将两个整数发送给服务器Server,同时服务器Server将两个整数相加后,再通过自定义的服务应答Response将两个整数的和反馈到客户端Client。

2.2服务数据的定义

在功能包目录下创建一个新的文件夹,命名为srv,并在此文件夹中创建一个空文件AddTwoInts.srv。

在AddTwoInts.srv文件中输入以下代码,定义服务消息内容。

1
2
3
4
int64 a
int64 b
---
int64 sum

说明:
使用“—-”作为分隔符,“—-”上面代表服务请求的消息内容,“—-”下面代表服务应答的消息内容。
相关资料:https://wiki.ros.org/srv

2.3创建.cpp文件

在功能包下面的src文件夹目录下创建一个空文件client.cpp。

2.4客户端编程

打开上面所创建的文件client.cpp,输入以下代码。

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
#include <cstdlib>
#include "ros/ros.h"
#include "communication_pkg/AddTwoInts.h"

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

//从终端获取数据
if(argc != 3)
{
ROS_INFO("usage: add_two_ints_client X Y");
return 1;
}

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

//创建一个客户端,定义服务请求数据类型communication_pkg::AddTwoInts
ros::ServiceClient client = n.serviceClient<communication_pkg::AddTwoInts>("add_two_ints");

//定义服务消息内容
communication_pkg::AddTwoInts 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;
}

说明:
◆头文件cstdlib.h是C++里面的一个常用函数库,等价于C中的stdlib.h,可以提供一些函数与符号常量。
◆头文件ros/ros.h包含了标准ROS类的声明,在每一个ROS程序中都需要包含它。
◆头文件communication_pkg/AddTwoInts.h是由AddTwoInts.srv编译扩展得到,包含了针对C++类的定义等,它存放在工作空间的devel/include路径下的communication_pkg文件夹中。
◆int main(int argc, char **argv)是main函数的一种定义形式,其参数argc和argv用于运行时,把命令行参数传入主程序,其中arg是指arguments,即参数。
(1)int argc:英文名为arguments count(参数计数),表示运行程序传送给main函数的命令行参数总个数,包括可执行程序名,其中当argc=1时表示只有一个程序名称,此时存储在argv[0]中。
(2) char **argv:英文名为arguments value/vector(参数值),用来存放指向字符串参数的指针数组,每个元素指向一个参数,空格分隔参数,其长度为argc。数组下标从0开始,argv[argc]=NULL。argv[0]指向程序运行时的全路名;argv[1]指向程序在DOS命令中执行程序名后的第一个字符串;argv[2]指向执行程序名后的第二个字符串。
◆ros::init(argc, argv, “add_two_ints_client”)的作用是初始化ROS节点,第三个参数表示节点名称,这个节点名是唯一的。
◆if(argc != 3){…}表示当终端命令行输入的参数数量不等于3,则执行这段代码。
◆ros::NodeHandle n;的作用是创建句柄,启动ROS节点。
◆ros::ServiceClient client = n.serviceClient(“add_two_ints”);这是用户调用ROS服务的其中一种方法。它创建了一个客户端,其返回的ros::ServiceClient类的对象client之后将用于向ROS网络中名为add_two_ints的节点发送服务请求,服务请求的数据类型为communication_pkg::AddTwoInts。
1
2
3
communication_pkg::AddTwoInts srv;
srv.request.a = atoll(argv[1]);
srv.request.b = atoll(argv[2]);

◆这是创建服务请求和服务应答的其中一种方法。它定义了一个service_example::AddTwoInts类型的对象,该对象中的成员正是srv文件中定义的a、b、sum,将终端命令行输入的两个数填充到数据成员a、b中。atoll()函数的作用是将字符串转换成长长整型数(long long int),即srv.request.a = atoll(argv[1])是将命令行输入的字符串中的第一个数转换成长长整型数,并赋值给成员a,srv.request.b = atoll(argv[2])则同理。
◆if(client.call(srv)){…}表示向ROS网络发起服务请求。由于服务请求处于阻塞状态,需要等待请求发起完成后才能返回值。如果服务调用成功,则call()将返回true,并且srv.response中的值将有效。如果调用失败,则call()将返回false,并且srv.response中的值将无效。
◆ROS_INFO()和ROS_ERROR()用于输出日志信息,日志消息分为五个不同的严重级别,按照严重性程度递增,分别为DEBUG、INFO、WARN、ERROR、FATAL。
综上所述,实现一个客户端的步骤大致可分为以下几点:
(1)初始化ROS节点;
(2)创建client实例;
(3)发布服务请求数据;
(4)等待Server处理之后的应答结果。

2.5服务器编程

在src文件夹下再创建一个空文件server.cpp,输入以下代码。

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
#include "ros/ros.h"
#include "communication_pkg/AddTwoInts.h"

//服务回调函数,输入为服务请求Req,输出为服务应答Res
bool add(communication_pkg::AddTwoInts::Request &req,
communication_pkg::AddTwoInts::Response &res)
{
//将两个数相加,并将结果存放在变量sum中
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)
{
//初始化ROS节点
ros::init(argc, argv, "add_two_ints_server");

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

//创建服务器,登记回调函数
ros::ServiceServer service = n.advertiseService("add_two_ints", add);

//循环等待
ROS_INFO("Ready to add two ints.");
ros::spin();

return 0;
}

说明:
◆main函数中一开始都是类似的,初始化ROS节点,创建句柄,从而启动ROS节点。
◆ros::ServiceServer service = n.advertiseService(“add_two_ints”, add);的作用是创建服务,并将服务加入到ROS网络中。这个服务在ROS网络中以add_two_ints命名,并且作为唯一标识,以便于其他节点通过服务名称进行请求。当接收到服务请求后,则调用add()回调函数。
◆ros::spin()的作用是让程序进入自循环的挂起状态,从而让程序以最好的效率接收客户端的请求并调用回调函数。
◆服务回调函数的表示形式如下:
1
bool callback(MReq &request, MRes &response)

其中MReq和MRes与提供给advertiseService()的请求/应答的数据类型相匹配。回调函数的返回值若为true,则表示服务请求成功,并且服务应答已填充必要的数据。若返回值为false则表示服务请求失败,并且服务应答将不会发送给客户端。
◆回调函数add(communication_pkg::AddTwoInts::Request &req, communication_pkg::AddTwoInts::Response &res)的作用是实现两个int型整数求和的服务,两个整数从request获取,求和结果填充到response里,request与response的具体数据类型在srv文件中被定义。
综上所述,实现一个服务器的步骤大致可分为以下几点:
(1)初始化ROS节点;
(2)创建Server实例;
(3)循环等待服务请求,进入回调函数;
(4)在回调函数中完成服务功能的处理,并反馈应答数据。
附相关资料:
(1)http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28c%2B%2B%29
(2)http://wiki.ros.org/roscpp/Overview/Services

3.配置与编译

3.1在CMaKeLists.txt中添加编译选项

打开功能包中的CMaKeLists.txt文件,在如下位置的find_package中添加功能包,以便于(节点)调用它们生成消息。

在如下位置添加相关的.srv文件,确保了CMake在重新配置时知道这些新添加的.srv文件,同时添加.srv文件在生成消息时的所有依赖项(功能包)。

1
add_service_files(FILES AddTwoInts.srv)


将如下位置中CATLIN_DEPENDS前面的“#”去掉。

在如下位置进行配置,add_executable(client src/client.cpp)的作用是将src文件夹下的client.cpp文件编译成名为client的可执行文件。target_link_libraries(client ${catkin_LIBRARIES})的作用是将client可执行文件与ROS相关的库链接。add_dependencies(client ${PROJECT_NAME}_gencpp)的作用是将client可执行文件与一些动态生成的文件链接。
1
2
3
4
5
6
7
add_executable(server src/server.cpp)
target_link_libraries(server ${catkin_LIBRARIES})
add_dependencies(server ${PROJECT_NAME}_gencpp)

add_executable(client src/client.cpp)
target_link_libraries(client ${catkin_LIBRARIES})
add_dependencies(client ${PROJECT_NAME}_gencpp)

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

打开功能包中的package.xml文件,在如下位置添加功能包依赖。

1
2
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>


说明:
1
<buildtool_depend>catkin</buildtool_depend>

这条语句表示需要依赖catkin工具包用于使用catkin_make指令进行编译。
1
<build_export_depend>roscpp</build_export_depend>

这条语句表示编译后导出相关文件需要依赖roscpp功能包。
1
<build_depend>message_generation</build_depend>

这条语句表示在编译时会依赖一个动态产生message的功能包。
1
<exec_depend>message_runtime</exec_depend>

这条语句表示在运行时会依赖message_runtime的功能包。

3.3编译文件

在/ROS_ws文件夹路径下打开一个新的终端,输入以下代码进行编译。

1
$ catkin_make


编译完成后,输入以下代码运行主节点。
1
$ roscore


打开一个新的终端,配置环境变量后,输入以下代码运行节点。
1
$ rosrun communication_pkg client 整数1 整数2


如果rosrun指令后面缺少需要相加的两个数,则会报如下错误。

打开一个新的终端,配置环境变量后,输入以下代码运行节点。
1
$ rosrun communication_pkg server


若想停止运行,关闭终端,使用快捷键Ctrl+c即可

ROS话题通信的编程实现

发表于 2019-12-29 | 分类于 ROS机器人操作系统

1.创建功能包

在ROS工作空间ROS_ws的src文件夹目录下创建一个功能包,命名为test_pkg,并编译完成。

2.节点编程与消息定义

2.1案例说明

定义一个发布者Publisher,通过自定义的消息Person将消息数据name、sex及age等发送到订阅者Subscriber,同时订阅者将消息内容打印到终端界面。

2.2话题消息的定义

在功能包目录下创建一个新的文件夹,命名为msg,并在此文件夹中创建一个空文件Person.msg。可以使用鼠标右键New Document>Empty Document,或在src路径下通过终端命令符touch创建空文件。

在Person.msg文件中输入以下代码,定义话题消息。

1
2
3
4
5
6
7
string name
uint8 sex
uint8 age

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

2.3创建.cpp文件

在功能包下面的src文件夹目录下创建一个空文件Publisher_test.cpp。

2.4话题发布者编程

打开上述创建的文件Publisher_test,输入以下代码。

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
#include "ros/ros.h"
#include "test_pkg/Person.h"

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

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

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

//设置消息发布频率
ros::Rate loop_rate(1);

int count = 0;
while(ros::ok())
{
//初始化消息内容
test_pkg::Person person_msg;
person_msg.name = "Tom";
person_msg.age = 18;
person_msg.sex = test_pkg::Person::male;

//发布消息
person_info_pub.publish(person_msg);

ROS_INFO("Publish Person Info: name:%s age:%d sex:%d",
person_msg.name.c_str(), person_msg.age, person_msg.sex);

//按照设置的频率延时
loop_rate.sleep();

}
}

说明:
◆头文件ros/ros.h包含了标准ROS类的声明,在每一个ROS程序中都需要包含它。
◆头文件test_pkg/Person.h是由Person.msg编译扩展得到,包含了针对C++类的定义等,它存放在工作空间的devel文件夹中。
◆main函数中ros::init(argc, argv, “Publisher_test”)的作用是初始化ROS节点,第三个参数表示节点名称,这个节点名是唯一的。
◆ros::NodeHandle n;的作用是创建句柄,启动ROS节点。
◆ros::Publisher person_info_pub = n.advertise(“/person_info”, 10);使用ros::Publisher类定义了一个发布者对象person_info_pub,发布的消息的数据类型为test_pkg::Person,用<>表示。/person_info表示发布消息数据的话题名,必须要与订阅者订阅的话题名相同。advertise()的第二个参数表示用于发布消息的消息队列长度,如果发布消息的速度快于底层响应的速度,则此处的数字指定在丢弃一些消息之前要缓冲多少条最新的消息。调用advertise()函数后,主节点(ROS Master)将通知任何试图订阅此话题名的节点,然后他们将与此节点协商对接。同时advertise()将返回一个Publisher对象,该对象使您可以通过调用publish()来发布有关该主题的消息。
◆ros::Rate loop_rate(1);的作用是设置消息发布频率为1Hz。
◆int count = 0;表明我们已发送多少条消息,用于为每个消息创建一个唯一的字符串。
◆若ros::ok()返回false,则可能发生了以下事件:
(1)SIGINT被触发(Ctrl-C);
(2)被另一同名节点踢出 ROS 网络;
(3)ros::shutdown()被程序的另一部分调用;
(4)节点中的所有ros::NodeHandles 都已经被销毁。
1
2
3
4
test_pkg::Person person_msg;
person_msg.name = "Tom";
person_msg.age = 18;
person_msg.sex = test_pkg::Person::male;

◆这部分的作用是对消息内容初始化,首先创建一个test_pkg::Person类的对象,之后设置这个对象中的name、age和sex。
◆person_info_pub.publish(person_msg);的作用是发布消息person_msg。
◆ROS_INFO()类似于C语言中的printf()函数,将内容打印在终端界面显示。
◆loop_rate.sleep();的作用是延时,以保证消息能够按照之前设置的发布频率发布出去。
综上所述,实现一个话题发布者的步骤大致可分为以下几点:
(1)初始化ROS节点;
(2)向ROS Master注册节点信息,包括发布的话题名和话题中的消息类型;
(3)按照一定频率循环发送消息。

2.5话题订阅者编程

在src文件夹下再创建一个空文件Subscriber_test.cpp,输入以下代码。

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
#include "ros/ros.h"
#include "test_pkg/Person.h"

//收到消息时进入回调函数
void personInfoCallback(const test_pkg::Person::ConstPtr& msg)
{
//打印消息
ROS_INFO("Subscribe Person Info: name:%s age:%d sex:%d",
msg->name.c_str(), msg->age, msg->sex);
}

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

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

//创建一个订阅者Subscriber
ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);

//循环接收消息
ros::spin();

return 0;
}

说明:
◆personInfoCallback(const test_pkg::Person::ConstPtr& msg)是一个回调函数,一旦订阅者接收到消息,则执行该回调函数。在该回调函数中,打印消息内容name、age和sex。
◆main函数中一开始跟发布者一样,初始化ROS节点,创建句柄。
◆ros::Subscriber person_info_sub = n.subscribe(“/person_info”, 10, personInfoCallback);使用ros::Subscriber类定义一个订阅者对象person_info_sub。subscribe()函数中的第一个参数表示需要订阅的话题名,第二个参数表示消息队列的长度,如果消息到达的速度快于处理速度,则开始丢弃最旧的消息之前将被缓冲的消息数。subscribe()函数的第三个参数是回调函数名,消息被传递到回调函数。
◆ros::spin()在调用后不会再返回,也就是主程序到这儿就不往下执行了,一般放在main函数最后,而不放在while()循环里。
综上所述,实现一个话题订阅者的步骤大致可分为以下几点:
(1)初始化ROS节点;
(2)订阅需要的话题;
(3)循环等待话题消息,接收到消息后进入回调函数;
(4)在回调函数中完成消息处理。
附相关资料:
(1) http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#Publisher.publish.28.29
(2) http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

3.配置与编译

3.1在CMaKeLists.txt中添加编译选项

打开功能包中的CMaKeLists.txt文件。在如下位置的find_package中添加message_generation功能包,以便于(节点)调用它们生成消息。

在如下位置添加相关的.msg文件,确保了CMake在重新配置时知道这些新添加的.msg文件,同时添加 .msg文件在生成消息时的所有依赖项(功能包)。

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


将如下位置中CATLIN_DEPENDS前面的“#”去掉。

在如下位置进行配置,add_executable(Publisher_test src/Publisher_test.cpp)的作用是将src文件夹下的Publisher_test.cpp文件编译成名为Publisher_test的可执行文件。target_link_libraries(Publisher_test ${catkin_LIBRARIES})的作用是将Publisher_test可执行文件与ROS相关的库链接。add_dependencies(Publisher_test ${PROJECT_NAME}_generate_messages_cpp)的作用是将Publisher_test可执行文件与一些动态生成的文件链接。
1
2
3
4
5
6
7
add_executable(Publisher_test src/Publisher_test.cpp)
target_link_libraries(Publisher_test ${catkin_LIBRARIES})
add_dependencies(Publisher_test ${PROJECT_NAME}_generate_messages_cpp)

add_executable(Subscriber_test src/Subscriber_test.cpp)
target_link_libraries(Subscriber_test ${catkin_LIBRARIES})
add_dependencies(Subscriber_test ${PROJECT_NAME}_generate_messages_cpp)

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

打开功能包中的package.xml文件,在如下位置添加功能包依赖。

1
2
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>


说明:
1
<build_depend>message_generation</build_depend>

这条语句表示在编译时会依赖一个动态产生message的功能包。
1
<exec_depend>message_runtime</exec_depend>

这条语句表示在运行时会依赖message_runtime的功能包。

3.3编译文件

在/ROS_ws文件夹路径下打开一个新的终端,输入以下代码进行编译。

1
$ catkin_make


编译完成后,输入以下代码运行主节点。
1
$ roscore


打开一个新的终端,首先配置环境变量,然后输入以下代码运行节点。
1
$ rosrun test_pkg Publisher_test


打开一个新的终端,首先配置环境变量,然后输入以下代码运行节点。
1
$ rosrun test_pkg Subscriber_test


若想停止运行,关闭终端,使用快捷键Ctrl+c即可。

4.话题可视化

打开一个新的终端,输入以下代码。

1
$ rqt_graph

由此可以得到如下的基于Qt的GUI界面,直观地看到话题通信的发布订阅节点和消息。

5.rostopic命令的使用

5.1

1
$ rostopic list

使用上述指令能够列出所有当前正在订阅和发布的话题,效果如下图。

5.2

1
$ rostopic list -v

使用上述指令能够得到当前正在订阅和发布的话题的详细内容介绍,效果如下图。

5.3

更多关于rostopic指令的使用,可以通过以下代码获取。

1
$ rostopic --help

ROS工作空间及功能包的创建

发表于 2019-12-26 | 分类于 ROS机器人操作系统

1.创建工作空间

1.1Ctrl + Alt + T打开一个新终端

1.2输入以下代码

1
$ mkdir -p ~/ROS_ws/src

说明:
mkdir是Linux命令,用于建立子目录。参数-p确保目录名称存在,不存在的就建一个。
这条语句的作用是在/home/用户名/下创建一个ROS工作空间,即文件夹ROS_ws。同时在工作空间ROS_ws下创建一个文件夹src。
你也可以将工作空间ROS_ws换成其他名字xxx,那么下面的所有命令中的ROS_ws都要对应换成xxx。

1.3输入以下代码

1
$ cd ~/ROS_ws/src

说明:
进入工作空间ROS_ws下的文件夹src。

1.4输入以下代码

1
$ catkin_init_workspace

说明:
对当前文件夹进行初始化,使它变成ROS的工作空间,同时src目录下会多出一个 CMakeLists.txt 文件。

1.5输入以下代码

1
$ cd ~/ROS_ws

或

1
$ cd ..

说明:
返回到上一级目录,即ROS_ws文件夹。

1.6输入以下代码

1
$ catkin_make install

说明:
对当前工作空间进行编译,编译完成后,工作空间ROS_ws中会再生成三个子目录:build、devel和install。

2.创建功能包

2.1输入以下代码

1
$ catkin_create_pkg test_pkg std_msgs roscpp rospy

说明:

1
$ catkin_create_pkg <功能包名称> [依赖的功能包1] [依赖的功能包2]...

这条语句的作用是创建功能包,代码语句的格式可参考上面。
test_pkg是功能包的名称,std_msgs、roscpp、rospy则是我们需要依赖的其他功能包。当我们使用C++或Python编写程序时,我们就需要依赖相关的库roscpp、rospy。我们在之后的例程中将运用到int、bool等消息类型,因此需要依赖相关的库std_msgs。

2.2输入以下代码

1
$ cd ~/ROS_ws

或

1
$ cd ..

说明:
返回到ROS_ws文件夹目录下。

2.3输入以下代码

1
$ catkin_make

说明:
对当前工作空间进行编译。

2.4输入以下代码

1
$ source devel/setup.bash

说明:
将对应的工作空间的路径加入到环境变量ROS_PACKAGE_PATH中。有些电脑系统环境是zsh,则最后的.bash需要改成.zsh,可以通过如下语句查看当前环境设定的sh类型:

1
$ echo $SHELL


如果新开了一个终端命令行,在使用该工作空间前,必须先将该工作空间的路径加入环境变量ROS_PACKAGE_PATH中,否则无法运行功能包。要想避免反复设置环境变量带来的麻烦,可以按照如下步骤配置。

2.4.1

进入主文件夹目录。

2.4.2

使用快捷键Ctrl + H,显示文件夹中的隐藏文件。

2.4.3

打开.bash文件,并拉到最后一行,添加如下语句:

1
$ source /home/用户名/ROS_ws/devel/setup.bash

2.5输入以下代码

1
$ echo $ROS_PACKAGE_PATH

说明:
ROS_PACKAGE_PATH是ROS本身的环境变量,它会根据这个环境变量查找所有功能包的路径。如果打印出以下信息,说明你的工作环境设置好了。

机器人操作系统(ROS)简介

发表于 2019-12-25 | 分类于 ROS机器人操作系统

1.ROS基本概念

ROS Wiki对于机器人操作系统(ROS)的解释是:ROS是一个适用于机器人的开源的元操作系统。它提供了操作系统应有的服务,包括硬件抽象,底层设备控制,常用函数的实现,进程间消息传递,以及包管理。它也提供用于获取、编译、编写、和跨计算机运行代码所需的工具和库函数。它的目的是为了提高机器人研发中的软件复用率,简化跨机器人平台创建复杂、鲁棒的机器人行为这一过程的难度与复杂度。

上图是ROS官方早期发布的一张图,简单定义了什么是ROS,即ROS是通讯机制、开发工具、应用功能以及生态系统的集合体,其中通信机制可以说是ROS的核心。

2.ROS架构

2.1OS层

ROS并非像Windows、Linux等传统意义上的操作系统,无法直接运行在计算机硬件上,因此它需要依赖于Linux系统。

2.2中间层

Linux系统本身没有针对机器人开发的中间件,因此ROS在中间层做了大量工作。首先是基于TCP/UDP网络,并在此之上进一步封装而构建的TCPROS/UDPROS通信系统。另外ROS还提供了一种进程内的通信方法——Nodelet,为多进程通信提供了一种更为优化的数据传输方式。在通信机制之上,ROS提供了大量机器人开发的库,以提供给应用层调用。

2.3应用层

应用层中ROS需要一个管理者——Master,负责管理整个系统正常运行,为节点间建立连接。

3.通信机制

ROS采用的是一种点对点的分布式通信机制,实现模块间点对点的松耦合连接,所有软件功能及工具都建立在这种通信机制上,为用户提供多节点(进程)的通信服务,其中ROS最核心的三种通信机制是话题(Topic)通信机制、服务(Service)通信机制和参数(Parameter)管理机制。

4.计算图

ROS系统的功能模块以节点为单位单独运行,可以分布于多个不同或相同的主机中,通过端对端的拓扑结构相连接。

4.1节点(Node)

节点就是执行具体任务的进程或独立运行的可执行文件,通常一个系统由多个节点组成,节点之间可以通过 ROS 客户端库(如roscpp 、rospy)相互通信。不同节点可使用不同编程语言,可分布式运行在不同的主机。节点在系统中的名称必须是唯一的。

4.2节点管理器(Master)

节点管理器在系统中主要起到了一个中介的作用,能够帮助节点相互找到彼此。节点管理器能够为节点提供命名和注册服务,以及跟踪和记录话题/服务通信。同时节点管理器还提供了一个参数服务器(Parameter Server),节点使用此服务器存储和检索运行时的参数。

4.3消息(Message)

消息本身是基于发布/订阅模型的话题通信机制而来的,具有一定的类型和数据结构,既包含ROS提供的标准数据类型,也可由用户自定义数据类型。

4.4话题(Topic)

话题通信采用的是一种异步通信机制。话题通信基于发布/订阅模型,数据由发布者传输给订阅者。其中节点既可以作为发布者发布消息,也可以作为订阅者订阅消息。同一个话题的发布者和订阅者可以不唯一,另外一个节点也可以发布或订阅多个消息。一般来说,发布者和订阅者并不知道对方的存在。发布者将信息发布在一个全局的工作区内,当订阅者发现该信息是它所订阅的,就可以接收到这个信息。通常用于数据传输。

4.5服务(Service)

服务通信采用的是一种同步通信机制。服务通信基于客户端/服务器模型,客户端(Client)发送请求数据(Request),服务器(Server)完成处理后返回应答数据(Response)。与话题不同的是,ROS中只允许有一个节点提供指定命名的服务。通常用于逻辑处理。

4.6动作(Action)

动作是基于ROS消息机制实现的一种问答通信机制,基于客户端/服务器模型,服务器可以连续反馈数据给客户端,客户端可以在任务运行过程中中止运行。动作Action的接口主要由goal、cancel、status、feedback和result组成,客户端可以在任务启动前向服务器发布任务目标goal,也可以在任务过程中向服务器发送cancel请求取消任务。服务器向客户端反馈服务器当前的状态,或周期性反馈任务运行的监控数据,而result在任务运行过程中只发布一次,仅在服务器完成动作后反馈一个最终结果。

4.7消息记录包(Bag)

消息记录包是一种用于保存和回放 ROS 消息数据的文件格式。它使用.bag格式保存消息、主题、服务和其他ROS数据信息,可以在事件发生后,通过使用可视化工具调用和回放数据,检查在系统中到底发生了什么。记录包文件可以像实时会话一样在ROS中再现情景,在相同时间向主题发送相同的数据。通常用于调试算法。

4.8参数(Parameter)

参数服务器能够保存一部分参数作为全局共享字典,系统中的所有节点都可以通过网络访问这些共享字典,存储和检索参数的参数名或参数值,而字典中的数据类型包含了int整型、float浮点型、string字符串等。参数的存储和检索采用的通信机制是更为底层的RPC,而不再是话题或服务。它更适合存储静态、非二进制的配置参数,不适合存储动态配置的数据。

4.9功能包(Package)

功能包是ROS软件中的基本单元,包含节点源码、配置文件、数据定义等。

4.10功能包清单(Package manifest)

功能包清单记录了功能包的基本信息,包含作者信息、许可信息、依赖选项、编译标志等。

4.11元功能包(Meta Package)

元功能包是一种特殊的功能包,它只包含元功能包清单文件。它的作用是将多个具有相同功能的功能包整合成一个逻辑上独立的功能包,类似于功能包集合。

5.开源社区

ROS开源社区级主要关于ROS资源,能够通过独立的网络社区分享软件和知识。这些资源包括:
发行版(Distribution)—— ROS发行版是可以独立安装的,带有版本号的一系列功能包集。ROS发行版像Linux发行版一样发挥类似的作用。这使得ROS软件安装更加容易,而且能够通过一个软件集合来维持一致的版本。
软件源(Repositorie)—— ROS依赖于共享开源代码与软件源的网站或主机服务,在这里不同的机构能够发布分享各自的机器人软件和程序。
ROS Wiki—— ROS Wiki是用于记录有关ROS系统信息的主要论坛。任何人都可以注册账户和贡献自己的文件,提供更正和更新,编写教程以及其他信息。
邮件列表(Mailing list)—— ROS用户邮件列表是关于ROS的主要交流渠道,能够交流从ROS软件更新到ROS软件使用中的各种疑问或信息。

<i class="fa fa-angle-left"></i>123<i class="fa fa-angle-right"></i>

chan

21 日志
6 分类
© 2021 chan
由 Hexo 强力驱动
|
主题 — NexT.Muse v5.1.4