ROS 技巧杂物间

概述

整理收集一些阅读源代码时看到的ROS技巧以及常见错误。

1. getPath()

这是在阅读Velodyne雷达节点代码时看到的，通过ros:📦:getPath()可以获取ROS package的目录，然后读取已放置好的校准文件。

// have to use something: grab unit test version as a default
        std::string pkgPath = ros::package::getPath("velodyne_pointcloud");
        config_.calibrationFile = pkgPath + "/params/64e_utexas.yaml";

2. Boost::Bind()

参考
简单的讲这个在ROS中主要用于多参数函数作为回调函数时使用。
如果定义函数

void func(int x, int y)
{
	//do something
}

使用bind时，_1表示x，_2表示y。

bind( func, 5, _1)(x);                     // func(5, x)
bind( func, _2, _1)(x, y);                 // func(y, x)
bind( func, _2, _2)(x, y);                 // func(y, y)

而在ROS下，用于subscribe回调，
1
定义

void func(const std_msgs::Int8::ConstPtr& msg);

那么

ros::Subscriber sub=nd.subscribe<std_msgs::Int8>("/test", 10, func）;//void func(const std_msgs::Int8::ConstPtr& msg);

以及
2
定义

void func(const std_msgs::Int8::ConstPtr& msg，int& x，int& y);

那么

ros::Subscriber sub=nd.subscribe<std_msgs::Int8>("/test", 10, boost::bind(&func, _1, index1, index2)）;//void func(const std_msgs::Int8::ConstPtr& msg，int& x，int& y);

可以看到对于不同的回调函数func，所采取的代码形式也不一样，这是因为参数多了，需要使用bind，而此时_1表示的是sub订阅的消息。注意:使用ConstPtr&不是Ptr&。
而对于多传感器同步时，同样有
3
定义

void callback(const sensor_msgs::ImageConstPtr& image_ptr, const sensor_msgs::ImuConstPtr& imu_mti_ptr,const sensor_msgs::ImuConstPtr& imu_sbg_ptr,  const sensor_msgs::PointCloud2ConstPtr& lidar_ptr) 

那么，这里的this是因为已经封装成为了类，表示类的成员函数，如果不使用类不需要this。

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,sensor_msgs::Imu,sensor_msgs::Imu,sensor_msgs::PointCloud2> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10),image_sub,imu_sub_mti,imu_sub_sbg,lidar_sub);//10
  sync.registerCallback(boost::bind(&Sync_sub_pub::callback,this,_1,_2,_3,_4));//

总结：_1,_2,_3,_4表示传入的参数，用于回调时这个参数可以从函数声明或者定义查看，稍稍不明显。

3. dynamic_reconfigure::Server

用于在参数服务器中参数发生变化时依旧能够动态更新参数。这个之后自己练习一下，先做个标记。动态重配置文件为.cfg，需要依赖
dynamic_reconfigure。参考

// Initialize dynamic reconfigure
  srv_ = boost::make_shared <dynamic_reconfigure::Server<velodyne_driver::
    VelodyneNodeConfig> > (private_nh);
  dynamic_reconfigure::Server<velodyne_driver::VelodyneNodeConfig>::
    CallbackType f;
  f = boost::bind (&VelodyneDriver::callback, this, _1, _2);
  srv_->setCallback (f); // Set callback function und call initially

4. 修改msg文件报错

自己定义msg文件，catkin_make没有问题，但是当自己修改了msg文件内容之后catkin_make就会出现错误

The dependencies of the message/service 'nlink_parser/LinktrackTagframe0' have changed. Please rerun cmake.

message文件错误，需要重新构建，但是我重新构建不行，查找之后发现需要

catkin clean 

我的kinetic版本不能执行这个命令，
查找也没有多少结果。参考

4.1 最终解决办法：

• 删除工作空间（我的在～/catkin_ws）中devel和build目录下对应的项目文件

重新catkin_make就可以了。

5. PCL配置错误

undefined reference to `pcl::console::print(pcl::console::VERBOSITY_LEVEL, char const*, …)’
这种错误一般是CMakeLists文件没有编写好。

5.1 解决办法

确保PCL库所有设置已经完整配置
我只需要在CMakeLists中添加

target_link_libraries(dev_detect
  ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_LIBRARIES}
)

完整的PCL配置为

# pcl
find_package( PCL REQUIRED)
include_directories( ${PCL_INCLUDE_DIRS} )
link_directories(${PCL_LIBRARY_DIRS})

target_link_libraries(dev_detect
  ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_LIBRARIES}
)

6. 雷达点云PointCloud2 pcl::PointCloud

数据格式转换参考代码
官方对点云格式的介绍，主要有四种，sensor_msgs::PointCloud已经弃用。参考

sensor_msgs::PointCloud — ROS message (deprecated)

sensor_msgs::PointCloud2 — ROS message

pcl::PCLPointCloud2 — PCL data structure mostly for compatibility with ROS (I think)

pcl::PointCloud<T> — standard PCL data structure 

sensor_msgs::PointCloud2 to pcl::PointCloudpcl::PointXYZ如果报错可以参考解决办法

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>

 void cloud_cb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& input){

    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*input,pcl_pc2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
    // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::fromROSMsg (*input, cloud);
    //do stuff with temp_cloud here
    }

sensor_msgs::PointCloud2与pcl::PCLPointCloud2转换并进行降采样。

#include <pcl/filters/voxel_grid.h>
...
void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;

  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  // Perform the actual filtering
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloudPtr);
  sor.setLeafSize (0.1, 0.1, 0.1);
  sor.filter (cloud_filtered);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 output;
  pcl_conversions::fromPCL(cloud_filtered, output);

  // Publish the data
  pub.publish (output);
}

sensor_msgs::PointCloud2与pcl::PointCloud转换并估计场景中找到的最大平面的平面系数

#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>

...

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg (*input, cloud);

  pcl::ModelCoefficients coefficients;
  pcl::PointIndices inliers;
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

  seg.setInputCloud (cloud.makeShared ());
  seg.segment (inliers, coefficients);

  // Publish the model coefficients
  pcl_msgs::ModelCoefficients ros_coefficients;
  pcl_conversions::fromPCL(coefficients, ros_coefficients);
  pub.publish (ros_coefficients);
}

7. Dropped 100.00% of messages so far.

运行Gmappping时出现错误提示，其他参数：话题等都设置设置正确了，但是程序不能正确运行。

MessageFilter [target=odom ]: Dropped 100.00% of messages so far.

7.1 tf树

使用命令可以看到当前的tf树,这个树展示了当前可以使用的坐标系转换关系。

rosrun rqt_tf_tree rqt_tf_tree

7.2 正常情况

正常情况下的tf树如下，可以看到有四个，其中/odom，/base_link ，/base_laser三个时必须的，表征了里程计坐标系，机器人坐标系，雷达坐标系之间的转换关系。

启动Gmapping之后会添加map坐标系，总共有五个坐标系，注意启动时设置激光雷达的话题

rosrun gmapping slam_gmapping scan:=base_scan

7.3 报错情况

报错的时候坐标系之间是不连续的，或者缺少的。
因此需要重新设置或者添加正确的坐标转换关系。
如下面坐标系转换关系不完整，因此还需要添加坐标系转换关系/odom -> /base_link。
具体的添加就具体情况具体分析吧。
参考链接Dropped 100.00% of messages so far.问题解决

8. ROS日志级别

ROS日志级别有五种: DEBUG, INFO, WARN, ERROR,FATAL.

8.1 查看方法

使用下面的命令可以查看具体的信息输出，还可以屏蔽，排序等操作

rosrun rqt_console rqt_console 可以查看信息输出

使用下面的命令可以设置每个节点输出信息的级别

rosrun rqt_logger_level rqt_logger_level 可以改变日志输出的级别

如下所示为更改节点slam_gmapping输出信息级别为Debug。
如下所示为节点slam_gmapping输出的信息，有INFO和DEBUG。

参考