服务和话题

rosservice call

rosservice call addTwoNum 3 4: 调用服务，addTwoNum是服务名称， 名称不加引号，3和4是服务中的req变量的值，注意必须按服务文件中的顺序赋值。

但上面是简单情况，srv文件稍微有点复杂时，就会怎么填也不对，个人认为这是ROS Service很失败的一个地方。
例如srv文件的请求部分是这样的：

1
2

string request_string
string album

1
2

rosservice call service_name "request_string: ''
album: ''"

call执行成功时，终端不会有任何结果。但是经常出现这样的错误： ERROR: service [/control_cam] responded with an error: ，但程序执行没有问题。原因是 程序中对srv文件中的应答没有处理

其他常用命令：

• rosservice list: 显示当前所有活动的服务
• rosservice info addTwoNum: 显示服务的信息
• rosservice type addTwoNum: 显示服务的变量类型

其实服务的相关命令和话题的很类似

ROS的服务类型

启动乌龟节点时，有一个服务叫clear，类型：

1
2
3

rosservice type clear

std_srvs/Empty

std_srvs包还有两个服务：std_srvs/Trigger和std_srvs/SetBool，但是很不常用

• locate的速度比find快，因为它并不是真的查找文件，而是查数据库。

• 新建的文件，我们立即用locate命令去查找，一般是找不到的，因为数据库的更新不是实时的，而是每天

• locate命令所搜索的后台数据库在/var/lib/mlocate这个目录下，可能有些Linux系统位置不同，具体我们可以用locate locate查询

• 并不是所有的目录下的文件都会用locate命令搜索到，/etc/updatedb.conf这个配置文件中，配置了一些locate命令的一些规则。

• updatedb会大致每天运行，这是靠系统的crontab命令实现的

• updatedb -U：更新指定目录相关的数据库信息。默认是整个系统，耗时比较长，因此可以使用该参数，比如sudo updatedb -U /home/user/

updatedb的配置文件 /etc/updatedb.conf

1
2
3
4
5

cat /etc/updatedb.conf 
PRUNE_BIND_MOUNTS = "yes"
PRUNEFS = "9p afs anon_inodefs auto autofs bdev binfmt_misc cgroup cifs coda configfs cpuset debugfs devpts ecryptfs exofs fuse fuse.sshfs fusectl gfs gfs2 gpfs hugetlbfs inotifyfs iso9660 jffs2 lustre mqueue ncpfs nfs nfs4 nfsd pipefs proc ramfs rootfs rpc_pipefs securityfs selinuxfs sfs sockfs sysfs tmpfs ubifs udf usbfs fuse.glusterfs ceph fuse.ceph"
PRUNENAMES = ".git .hg .svn"
PRUNEPATHS = "/afs /media /mnt /net /sfs /tmp /udev /var/cache/ccache /var/lib/yum/yumdb /var/spool/cups /var/spool/squid /var/tmp /var/lib/ceph"

• PRUNENAMES 搜索时不搜索的文件类型
• PRUNEPATHS 搜索时不搜索的路径
• PRUNE_BIND_MOUNTS = "yes" 开启搜索限制
• PRUNEFS 搜索时不搜索的文件系统

locate 常用命令

1
2
3
4
5
6
7

locate -c   # 查询指定文件的数目。(c为count的意思)
locate -e   # 只显示当前存在的文件条目。(e为existing的意思)
locate -i   # 查找时忽略大小写区别

# 使用正则表达式查找文件
locate -r makefile$		# 以makefile结尾的文件
locate -r ^/home/user/		# 以/home/user/开头的文件		

从结果中取出词尾是config2的

注意：locate的结果可能是不存在的文件，这时最好用locate -e

locate 查找文件tree.xml，也就是知道完整的文件名时，那么最好用 locate /tree.xml，如果不加/，会显示test_tree.xml的结果

find 常用命令

查找当前目录中(包括子目录)所有扩展名为cfg的文件:

1
2

# 或者 '*.cfg'
find -name *.cfg

• find . -name '*.cpp' -mmin -30 当前目录下，最近30分钟修改的cpp文件

• find . -name '*.cpp' -mtime 0 当前目录下，最近24小时修改的cpp文件

• find . -type f -mtime 0 当前目录下，最近24小时修改的常规文件

消息类型优化

AprilTagDetectionArray消息的定义过于繁杂，构成如下图：

其中常见的是geometry_msgs/Pose，它包括:

1
2

geometry_msgs/Point position
geometry_msgs/Quaternion orientation

其实我们需要的只有id, size和geometry_msgs/Pose，所以可以定义AprilTagDetection.msg如下:

1
2
3

int32[] id
float64[] size
geometry_msgs/Pose  pose

需要修改代码部分，主要是函数TagDetector::detectTags

CPU占用大的问题

一般情况下，程序占CPU近100%，消耗CPU的代码就是函数TagDetector::detectTags，调用在:

1
2

tag_detections_publisher_.publish(
      tag_detector_->detectTags(cv_image_,camera_info));

将apriltag_detector_detect函数换上april3版本后，CPU占用只降了一点，还是超过90%。

优化方法：

• 把setting.yaml中的tag_debug设置为0，直接将程序占用的CPU直接降到了45%左右
• 将tag_refine_pose设置为0，会大大降低CPU占用率
• tag_blur保持为0，小于0时会稍微增加CPU占用率
• tag_threads用默认值即可，不能太大，否则CPU会飙升
• 更换主板而不是CPU

启动节点所用的launch文件如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

<launch>
  <arg name="node_namespace" default="apriltags2_ros_continuous_node" />
  <arg name="camera_name" default="/usb_cam" />
  <!-- 摄像头的坐标系名称 -->
  <arg name="camera_frame" default="camera" />
  <arg name="image_topic" default="image_raw" />

  <!-- 加载两个yaml -->
  <rosparam command="load" file="$(find apriltags2_ros)/config/settings.yaml" ns="$(arg node_namespace)" />
  <rosparam command="load" file="$(find apriltags2_ros)/config/tags.yaml" ns="$(arg node_namespace)" />
  
  <node pkg="apriltags2_ros" type="apriltags2_ros_continuous_node" name="$(arg node_namespace)" clear_params="true" output="screen" >
    <!-- 将代码中的话题名称remap为自定义的 -->
    <remap from="camera_info" to="$(arg camera_name)/camera_info" />
    <param name="camera_frame" type="str" value="$(arg camera_frame)" />
    <!-- 可以设定为false -->
    <param name="publish_tag_detections_image" type="bool" value="true" />
  </node>
</launch>

话题

摄像头节点usb_cam是Apriltags2_ros必备的，前者为后者提供了图像，两个节点的关系如下图:

摄像头节点usb_cam发布两个话题:

• /usb_cam/image_raw: 消息类型sensor_msgs/Image，包含不失真的图像，实际是一个视频流。
• /usb_cam/camera_info: 消息类型sensor_msgs/CameraInfo，包括了摄像头的校正矩阵等信息，不必深入研究。

apriltags2_ros节点发布三个话题:

• tf: 摄像头坐标系和tag坐标系之间的转换，需要在setting.yaml中设置publish_tf:true
• tag_detections: 带有tag ID,size,Pose等关键信息
• tag_detections_image: 摄像头画面，但将检测到的tag高亮。需要在setting.yaml中设置publish_tag_detections_image==true，本话题可以不发布

settings.yaml

• tag_family (string, default: tag36h11) //用于检测的apriltags家族。支持：tag36h11 , tag36h10 , tag25h9 , tag25h7 和 tag16h5。

• tag_border (int, default: 1) //apriltags码外部（圆周）黑色边框的宽度

• tag_threads (int, default: 4) //AprilTag 2核心算法应该使用多少个线程

• tag_decimate (double, default: 1.0) //四边形的检测可以在低分辨率图像上进行，以牺牲姿态精度和略微降低检测率来提高速度

• tag_blur (double, default: 0.0) //大于0时会模糊图片，小于0时会锐化图片

• tag_refine_edges (int, default: 1) //提高对tag的检测fit、角落的精度、姿态的精度，建议设置为1

• tag_refine_decode (int, default: 0) //值为1时，可以减少错误检测率

• tag_debug(bool, default: false) //设置为真会将算法的中间图形保存到 ~/.ros 目录，这对追踪失效的检测很有用，但只能用于single image detector

• tag_refine_pose (int, default: 0) //值大于0时，可以提高姿态的精度，但会大大增加CPU占用

• publish_tf (bool, default: false) //是否发布tf话题上的apriltagsID和摄像头的相对位姿

• camera_frame (string, default: camera) //摄像头坐标系的名字

• publish_tag_detections_image (bool, default: false) //是否发布到tag_detections_image话题上

tags.yaml

对于单个tag，只需要做如下设置：

1
2
3
4
5
6

standalone_tags:
  [
    {id: 0, size: 0.093},
    {id: 8, size: 0.15},
    {id: 9, size: 0.093}
  ]

源码分析

从launch文件可以看出节点的源码是apriltags2_ros_continuous_node.cpp：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

#include <ros/ros.h>
#include <nodelet/loader.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "apriltags2_ros_me");
  nodelet::Loader nodelet;
  nodelet::M_string remap(ros::names::getRemappings());
  nodelet::V_string nargv;

  nodelet.load(ros::this_node::getName(),
              "apriltags2_ros/ContinuousDetector",
              remap, nargv);
  ros::spin();
  return 0;
}

ContinuousDetector类如下：

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

ContinuousDetector::ContinuousDetector ()
{
  ROS_INFO("ContinuousDetector  construct");
}

void ContinuousDetector::onInit ()
{
  ros::NodeHandle& nh = getNodeHandle();
  ros::NodeHandle& pnh = getPrivateNodeHandle();

  tag_detector_ = std::shared_ptr<TagDetector>(new TagDetector(pnh));
  draw_tag_detections_image_ = getAprilTagOption<bool>(pnh, 
      "publish_tag_detections_image", false);
  it_ = std::shared_ptr<image_transport::ImageTransport>(
      new image_transport::ImageTransport(nh));

  camera_image_subscriber_ =
      it_->subscribeCamera("/usb_cam/image_raw", 1,
                          &ContinuousDetector::imageCallback, this);

  tag_detections_publisher_ =
      nh.advertise<AprilTagDetectionArray>("tag_detections", 1);
  if (draw_tag_detections_image_)
  {
    tag_detections_image_publisher_ = it_->advertise("tag_detections_image", 1);
  }
}

void ContinuousDetector::imageCallback (
    const sensor_msgs::ImageConstPtr& image_rect,
    const sensor_msgs::CameraInfoConstPtr& camera_info)
{
  // Convert ROS's sensor_msgs::Image to cv_bridge::CvImagePtr in order to run
  // AprilTags 2 on the iamge
  try
  {
    cv_image_ = cv_bridge::toCvCopy(image_rect,
                                    sensor_msgs::image_encodings::BGR8);
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }
  // Publish detected tags in the image by AprilTags 2
  tag_detections_publisher_.publish(
      tag_detector_->detectTags(cv_image_,camera_info));
  // Publish the camera image overlaid by outlines of the detected tags and
  // their payload values
  if (draw_tag_detections_image_)
  {
    tag_detector_->drawDetections(cv_image_);
    tag_detections_image_publisher_.publish(cv_image_->toImageMsg());
  }
}

ROS启动乌龟追乌龟程序用的是roslaunch turtle_tf turtle_tf_demo.launch，其内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

<launch>

  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
  <!-- Axes -->
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>
  <!-- 针对两个乌龟的tf广播 -->
  <node name="turtle1_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle1" />
  </node>
  <node name="turtle2_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle2" />
  </node>

  <node name="turtle_pointer" pkg="turtle_tf" type="turtle_tf_listener.py" respawn="false" output="screen" >
  </node>

</launch>

乌龟的程序,按下方向键,乌龟只走一段距离,是在cmd_vel的回调函数里更新乌龟的位置,取决于发布的twist消息和更新间隔dt,由Qt实现

tf包处理的是任一个点在所有坐标系之间的坐标变换问题，它把各种转换关系建立在一个树结构上，树的每个节点是坐标系，每个坐标系可以有多个child，但只能有一个parent，转换只能是从parent向child。比如Tb-a表示坐标系a向b转换，也就是说a是parent，b是child，这个变换描述的就是child坐标系中的点在parent坐标系下的姿态。要实现这个变换，就是用child坐标系在parent坐标系下的描述（一个矩阵）去描述（乘以）这个点在child坐标系下的描述（坐标）。world参考系是tf树最顶端的父参考系

如果打算用tf解决你的坐标变换问题，请一定要先清晰的画出这棵树的结构，再开始写程序。比较重要的类是tf::TransformBroadcaster, tf::TransformListener, tf::Transform, tf::StampedTransform

在tf的运行机制中，由于tf会把监听到的内容放到一个缓存中。我们通过transformPose获取变换关系，是通过查询这个缓存来实现的。获取的数据不能保证实时性，会有一定的延迟。也有可能无法获得，因此这个函数在运行过程中会抛出异常，所以这里使用try-catch语句捕获这个异常并返回。

tf::TransformBroadcaster类

sendtransform接口可以建立tf树，发布一个从已有的父坐标系到新的子坐标系的变换时，这棵树就会添加一个树枝，之后就是维护。TransformBroadcaster类就是一个publisher, 如果两个frame之间发生了相对运动，TransformBroadcaster类就会发布TransformStamped消息到tf话题，当多个节点向tf话题发消息时，就形成了tf树。

tf::Transform

建立坐标系之间的位移和旋转的关系，最后用于sendTransform函数。

它是一个坐标转换。成员有：Matrix3x3 m_basis，用3*3的矩阵表示旋转； Vector3 m_origin，用3*1的向量表示平移。

tf::Transform支持乘法运算符，实际的计算是先把旋转矩阵和平移量组合为变换矩阵，变换矩阵相乘后，再转换为tf::Transform类型

tf::Transform类的重要函数如下：

1
2
3
4
5

Matrix3x3 &   getBasis ()    //Return the basis matrix for the rotation
const Vector3 & getOrigin ()  //Return the origin vector translation
Quaternion  getRotation ()  //Return a quaternion representing the rotation
Transform   operator* (const Transform &t) const  //Return the product of this transform and the other.
Transform   inverse ()    //Return the inverse of this transform

tf::StampedTransform类继承自tf::Transform，它多了两个重要变量就是child_frame_id_和frame_id_。

tf::TransformListener

监听一个父坐标系到子坐标系的变换，waitForTransform是监听转换关系，可以指定监听的时间或一直阻塞；lookupTransform紧随其后，获取 tf::Transform

使用前需要#include <tf/transform_listener.h>

TransformListener构造函数有两个，常用的是

1
2
3
4

TransformListener::TransformListener(
    ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME),  
    bool spin_thread = true 
)

参考我写的程序test_costmap。开始，如果没有map—->base_link的TF转换，则报错No Transform available Error。此时发布TF变换，则不再报错。然后再关闭TF变换，test_costmap还能正常运行10s，然后报错 Extrapolation Error

transformPose

原型是void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const，

target_frame就是你要把源pose转换成哪个frame上的pose。假如你的源pose的frame_id是”odom”，你想转到”map”上，那么target_frame写成“map”就可以了。stamped_in就是源pose，而stamped_out就是目标数据了，也就是转换完成的数据。需要注意的是，从参数上来看，转换时是不需要指定源frame_id的，这是因为它已经包含在了stamped_in中，换句话说，就是这个函数一个隐含的使用条件是，stamped_in中必须指明源pose属于哪个frame

把odom坐标系的数据转换到map坐标系下

1
2
3
4
5
6
7
8
9
10
11
12
13

geometry_msgs::PoseStamped pose_odom;
pose_odom.header = odom->header;
pose_odom.pose = odom->pose.pose;

geometry_msgs::PoseStamped pose_map;
try{
    listener.transformPose("map", pose_odom, pose_map);
}
catch( tf::TransformException ex)
{
    ROS_WARN("transfrom exception : %s",ex.what());
    return;
}

有时会出现这样的报错： transfrom exception : “map” passed to lookupTransform argument target_frame does not exist ，但是使用tf_echo发现是正常的。需要检查代码是不是在回调函数里运行了， 不需要在回调函数里创建TransformListener对象， 将它作为类成员变量或者全局变量。

全局变量是在main函数之前完成构造函数的，如果用到的类构造函数用到NodeHandle，就会报错。比如tf::TransformListener，解决方法是用全局指针，比如boost::shared_ptr<T>，然后在main函数的ros::init()之后指向一个对象。

参考：tf::TransformListener::transformPose [exception] target_frame does not exist

充电桩的导航，一般方案是先导航到充电桩附近的一个固定点，然后切换到另一种更精确的导航方式进行精确对接充电桩。 精确对接的定位技术有红外、蓝牙、激光特征、二维码、反光板

这里只谈Apriltag，如果是aruco二维码，使用aruco_ros

ROS官方提供了一个名为apriltags2_ros的包处理AprilTag，可以根据Tag进行定位导航，我使用的是april.launch。使用前需要先启动摄像头的节点，我用的是usb_cam.launch，如果摄像头程序重启，april.launch也要重启，否则无效。

节点会发布话题tag_detections和tag_detections_image，后者是带数字识别的图，前者是坐标关系的输出。默认的话题频率应该能达到10Hz，但是完全取决于计算机的性能，无法用参数配置。

settings.yaml中的参数如下，按默认即可:

• Make sure tag_border corresponds to your tags’ black border bit width (typically 1 - each square in the tag is represents 1 bit);

• Setting tag_threads allows certain parts of the core AprilTag 2 algorithm to run parallel computations. Typical multithreading advantages and limitations apply;

• 增大tag_decimate>1 allows to decimate (reduce the resolution) of the image by that amount. This makes the core AprilTag 2 algorithm faster (there are less pixels to process) but means that smaller tags are less likely to be detected，甚至无法识别。Pose estimation and decoding still happens with the full-resolution image;

• Setting tag_blur>0 blurs the image and tag_blur<0 sharpens the image;

• Setting tag_refine_edges=1 improves the detection fit to the tag, thus the corner detection precision, thus the pose estimation accuracy. Not computationally expensive and recommended to be on (1);

• Setting tag_refine_decode=1 reducecs the false negative detetion rate;

• Setting tag_refine_pose=1 improves the estimated pose accuracy but is computationally expensive;

这个包有自定义的消息文件，不知道为什么，如果在自己的程序(我的叫tag_nav)中想包含其消息的头文件，就会报错：

解决方法：先编译好apriltag2_ros包，然后注释掉CMakeLists中的generate_messages部分，再编译tag_nav。之后没事不要再动apriltag2_ros了。如果先注释再编译apriltag2_ros包，会无法在tag_nav中包含前者的头文件。

我们使用36H11规格的ID为9的二维码进行测试。

注意事项:

• 同一个tag ID不能对应不同的尺寸，这会造成检测的不确定。

• 同一张图案里不要有相同的tag ID

• standalone_tags 和 in tag_bundles 可以有相同ID的tag, 但是size也必须相同。

• 打印出的tags周围要有一些白色空间，这是AprilTag 2 算法要求的，所以我们最好使用带黑框的图案

两话题不同步的问题

运行后提示 image消息和camera_info消息没有同步

此时打开rqt，切换到主题tag_detections_image，发现没有结果

源码在continuous_detector.cpp，发现没有进入回调函数ContinuousDetector::imageCallback，它的注册在:

1

camera_image_subscriber_ =  it_->subscribeCamera("/usb_cam/image_raw",   1,  &ContinuousDetector::imageCallback, this);

既然二者目前没有同步，当然没有进入回调函数，也就无法识别Tag了。最终发现原因就是在工控机上运行了roslaunch usb_cam usb_cam.launch，而在远程机运行了apriltag_ros包，而两台电脑通信不畅，解决方法就是将二者都在工控机上运行。

现在执行rqt就可以看到识别的结果了，二维码中心是ID,终端上也出现了二维码ID等信息:

Tag的位姿

若要查看检测结果，执行：

1

rostopic　echo　/tag_detections

总结一下，摄像头坐标系和tag坐标系的关系如下（普遍适用）：

坐标系转换关系问题

apriltag2_ros包存在tag_9坐标系向camera坐标系的转换，在TagDetector::detectTags函数的最后。

接下来我们要在camera和base_link之间建立联系，这样就将所有坐标系关联起来了，在launch文件中添加:

1

<node args="-0.24 0 0.295 1.571 0 -1.571 /base_link /camera 10" pkg="tf" name="base_link_to_camera" type="static_transform_publisher"/>

启动所有launch文件后，使用rosrun tf view_frame命令生成pdf文件，查看得知，拥有全部坐标系的情况是这样的:

apriltags2_ros的bug

在apriltags2_ros启动一段时间后，会偶然的无法正常检测二维码，tag_detections话题没有结果，也没有图形识别的结果，但摄像头的节点正常。

多次观察tag_detections的发布情况，发现最快的一次是header.seq到1400的时候就出错了，有时是到3900的时候，始终不确定，难以找到原因。

因此尝试AprilTag3，同时把setting.yaml中的线程数改为4，再也没有发生这一bug

参考：
ROS image_transport
Using Apriltags with ROS
image_transport::CameraSubscriber Class