通过ROS控制真实机械臂(18) --- 构建 octocmap 用于碰撞检测和避障
师兄和同门在做SLAM的时候，经常会用到的 octomap ，也就是八叉树地图。octomap相比于点云地图来说大大减小了地图的存储空间。既然octomap可以用于导航和避障，那么自然也可以导入moveit！，作为机械臂路径规划过程中的障碍物，方便机械臂和障碍物之间进行碰撞检测。
这里首先要提一下Movelt！并没有整合了物体识别和环境建模这些模块，而是利用传感器采集的数据导入的。而我的想法就是通过双目相机的深度及RGB图像生成用于碰撞检测的 octomap，这些octomap也可以依据贝叶斯准则不断实时更新。这样机械臂就可以避开真实世界的障碍物了。
碰撞检测是运动规划的一大难题,我们需要对每个采样点做有效性判断。所以,运动规划需要提供一个高效的碰撞检测算法。虽然Movelt！需要从外部传感器获取点云或深度信息，但是碰撞检测的算法moveit！还是完美集成了FCL( Flexible Collision Library),可以非常快速地实现octomap的碰撞检测。这个算法在学习ROS的时候，好像就囫囵吞枣的使用过了，当时还是手动往moveit！中导入物体的model的。值得一提的是，对于场景中每个物体都进行碰撞检测是浪费时间的，于是我们在配置moveit！的时候曾经生成过ACM(Allowed Collision Matrix)来进行优化。
1.ROS中深度图像转换成点云地图PCL
如果不是在ROS中操作的话，需要安装PCL库，并进行一定的依赖安装和配置，可以参考如下官网连接https://github.com/OctoMap/octomap 和大佬博客 https://blog.csdn.net/LOVE1055259415/article/details/79911653
大佬提供都是通过读取.pcd文件然后转换成octomap并写入.ot等文件当中，对于咱ROS中的使用不太方便，需要通过ROS消息的方式订阅点云消息并转换成octomap然后发布导入moveit！中。而且我所使用的传感器是双目相机，最开始获取的数据是RGBD信息，所以首先需要的是深度图像转换成点云信息的方法。于是构建了一个depth2pointcloud包测试一下
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace cv;
using namespace std;
pcl::PointCloud::Ptr depth2cloud( cv::Mat rgb_image, cv::Mat depth_image )
{
# 相机参数
float f = 570.3;
float cx = 320.0, cy = 240.0;
pcl::PointCloud::Ptr cloud_ptr( new pcl::PointCloud () );
cloud_ptr->width = rgb_image.cols;
cloud_ptr->height = rgb_image.rows;
cloud_ptr->is_dense = false;
for ( int y = 0; y < rgb_image.rows; ++ y ) {
for ( int x = 0; x < rgb_image.cols; ++ x ) {
pcl::PointXYZRGB pt;
if ( depth_image.at(y, x) != 0 )
{
pt.z = depth_image.at(y, x)/1000.0;
pt.x = (x-cx)*pt.z/f;
pt.y = (y-cy)*pt.z/f;
pt.r = rgb_image.at(y, x)[2];
pt.g = rgb_image.at(y, x)[1];
pt.b = rgb_image.at(y, x)[0];
cloud_ptr->points.push_back( pt );
}
else
{
pt.z = 0;
pt.x = 0;
pt.y = 0;
pt.r = 0;
pt.g = 0;
pt.b = 0;
cloud_ptr->points.push_back( pt );
}
}
}
return cloud_ptr;
}
int main(int argc,char* argv[])
{
ros::init (argc, argv, "publish_depth");
ros::NodeHandle nh;
cv::Mat depth;
cv::Mat image;
image=cv::imread("/home/redwall/catkin_ws/src/depth2pointcloud/data/rgb_index/144.ppm");
depth=cv::imread("/home/redwall/catkin_ws/src/depth2pointcloud/data/dep_index/144.pgm",IMREAD_ANYDEPTH);
string pcdName("/home/redwall/catkin_ws/src/pointcloud2octomap/data/testpcd.pcd");
if(!image.data||!depth.data) // 判断图片调入是否成功
{
cout<< "no image"<::Ptr cloud(new pcl::PointCloud);
cloud=depth2cloud(image,depth);
pcl::io::savePCDFileASCII(pcdName,*cloud);
cout<<"successful"< 上述ROS包通过读取RGB图片和 深度图像，利用pcl库方法转换成
2.ROS中深度图像直接转换成octomap
既然测试完毕，那就可以直接通过depth转换成octomap了，上面使用的方法同时使用了RGBD，转换成PointXYZRGB，简单起见，可以舍弃RGB信息，仅使用深度图像来生成
1.创建ROS包：depth2octomap
2.CMakeLists.txt和package.xml
cmake_minimum_required(VERSION 2.8.3)
project(depth2octomap)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
rostime
sensor_msgs
message_filters
cv_bridge
image_transport
)
find_package(PCL REQUIRED)
find_package(OpenCV REQUIRED)
find_package(OpenMP)
catkin_package()
include_directories(${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS})
add_executable (depth2octomap src/depth2octomap.cpp)
target_link_libraries (depth2octomap ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
depth2octomap
0.0.0
The depth2octomap package
crp
TODO
catkin
roscpp
rospy
std_msgs
cv_bridge
image_transport
roscpp
rospy
std_msgs
cv_bridge
image_transport
3.src/depth2octomap.cpp
#include
#include
#include
#include
#include
#include
// PCL 库
#include
#include
#include
#include
// 定义点云类型
typedef pcl::PointCloud PointCloud;
using namespace std;
using namespace cv;
// 相机内参
const double camera_factor = 1000;
const double camera_cx = 319.025;
const double camera_cy = 236.750;
const double camera_fx = 384.657;
const double camera_fy = 384.657;
// 全局变量：图像矩阵和点云
cv_bridge::CvImagePtr color_ptr, depth_ptr;
cv::Mat color_pic, depth_pic;
/*** RGB处理 ***/
void color_Callback(const sensor_msgs::ImageConstPtr& color_msg)
{
try
{
color_ptr = cv_bridge::toCvCopy(color_msg, sensor_msgs::image_encodings::BGR8);
cv::waitKey(1050); // 不断刷新图像，频率时间为int delay，单位为ms
}
catch (cv_bridge::Exception& e )
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", color_msg->encoding.c_str());
}
color_pic = color_ptr->image;
}
/*** Depth处理 ***/
void depth_Callback(const sensor_msgs::ImageConstPtr& depth_msg)
{
try
{
depth_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_32FC1);
cv::waitKey(1050);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from '%s' to 'mono16'.", depth_msg->encoding.c_str());
}
depth_pic = depth_ptr->image;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "publish_octomap");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("/camera/color/image_raw", 1, color_Callback);
image_transport::Subscriber sub1 = it.subscribe("/camera/depth/image_rect_raw", 1, depth_Callback);
ros::Publisher pointcloud_publisher = nh.advertise("/pointcloud/output", 1);
// 点云变量
PointCloud::Ptr cloud ( new PointCloud );
sensor_msgs::PointCloud2 pub_pointcloud;
double sample_rate = 1.0; // 1HZ
ros::Rate naptime(sample_rate); // use to regulate loop rate
while (ros::ok()) {
// 遍历深度图
for (int m = 0; m < depth_pic.rows; m++){
for (int n = 0; n < depth_pic.cols; n++){
// 获取深度图中(m,n)处的值
float d = depth_pic.ptr(m)[n];//ushort d = depth_pic.ptr(m)[n];
// d 可能没有值，若如此，跳过此点
if (d == 0)
continue;
// d 存在值，则向点云增加一个点
pcl::PointXYZRGB p;
// 计算这个点的空间坐标
// p.z = double(d) / camera_factor;
// p.x = (n - camera_cx) * p.z / camera_fx;
// p.y = (m - camera_cy) * p.z / camera_fy;
// 相机模型是垂直的
p.x = double(d) / camera_factor;
p.y = -(n - camera_cx) * p.x / camera_fx;
p.z = -(m - camera_cy) * p.x / camera_fy;
// 从rgb图像中获取它的颜色
// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
p.b = color_pic.ptr(m)[n*3];
p.g = color_pic.ptr(m)[n*3+1];
p.r = color_pic.ptr(m)[n*3+2];
// 把p加入到点云中
cloud->points.push_back( p );
}
}
// 设置并保存点云
cloud->height = 1;
cloud->width = cloud->points.size();
ROS_INFO("point cloud size = %d",cloud->width);
cloud->is_dense = false;// 转换点云的数据类型
pcl::toROSMsg(*cloud,pub_pointcloud);
pub_pointcloud.header.frame_id = "camera_link";
pub_pointcloud.header.stamp = ros::Time::now();
// 发布合成点云
pointcloud_publisher.publish(pub_pointcloud);
// 清除数据并退出
cloud->points.clear();
ros::spinOnce(); //allow data update from callback;
naptime.sleep(); // wait for remainder of specified period;
}
} 4.上面的depth2octomap节点已经将深度转换成点云了，至于怎么转换成octomap还需要通过launch来启动octomap_server节点：octomaptransform.launch
3.Realsense深度转octomap
刚写这篇博客的时候，还没有相机在手上，结果刚过两天，老板的realsense D435就发货了，花了两天时间在ROS中完成了相机的启动，https://blog.csdn.net/qq_34935373/article/details/104891420 ，通过相机发布RGBD信息，然后用上面的包转换成octomap，其中还有一些细节需要关注：
- 相机参数矩阵获取：fx、fy、cx、cy，realsense提供了获取这些参数的API，可以查看出厂的标定参数https://github.com/IntelRealSense/librealsense/issues/5239
- 计算点云在空间中的坐标的时候，因为realsense发布的带模型的launch的时候，由于模型在RViz中式水平放置的，所以坐标需要调整成x方向，而不是原本的z方向，因此就有如下调整：
// 计算这个点的空间坐标
//p.z = double(d) / camera_factor;
//p.x = (n - camera_cx) * p.z / camera_fx;
//p.y = (m - camera_cy) * p.z / camera_fy;// 相机模型是垂直的
p.x = double(d) / camera_factor;
p.y = -(n - camera_cx) * p.x / camera_fx;
p.z = -(m - camera_cy) * p.x / camera_fy; - 通过配置octosever修改点云空间范围
-
# 启动相机节点，发布RGBD信息 $ roslaunch realsense2_camera rs_camera.launch # 订阅RGBD信息，转换成点云并发布成octomap $ roslaunch depth2octomap octomaptransform.launch下面两幅图分别是点云图和八叉树图：
4.TODO
当然，在获取点云数据时 ，由于设备精度，操作者经验环境因素带来的影响，被测物体表面性质变化和数据拼接配准操作过程的影响，点云数据中讲不可避免的出现一些噪声。所以还需要在上述文件中根据需要加上点云滤波，离群点舍弃等操作。参考大神：https://blog.csdn.net/qq_34719188/article/details/79179430
Moveit！通过场景规划Planning_scene导入octomap
和之前的方法差不多类似，只不过应用的场景变成了moveit！当中，通过使用深度相机感知环境数据，感知后的环境可以作为环境障碍物，机械臂规划路径时会主动避开有octomap地图的地方。官网配置http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/planning/src/doc/perception_configuration.html，其中用到了moveit的组件 Occupancy Map Updater，Occupancy Map Updater 接受两种数据：
- The PointCloud Occupany Map Updater: which can take as input point clouds (sensor_msgs/PointCloud2)
- The Depth Image Occupancy Map Updater: which can take as input Depth Images (sensor_msgs/Image)
1. 和之前配置moveit控制器的时候的套路是类似的，找到redwall_arm_moveit_config包中的config文件夹(官方使用的是PR2包，或者直接使用自己的包)，新建一个sensors_3d.yaml（文件名随意），写入如下内容：
# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
point_cloud_topic: /camera/depth/color/points
max_range: 5.0
frame_subsample: 1
point_subsample: 1和官方的配置有所不同，因为我是用的是realsense深度传感器，可在话题/camera/depth/color/points上发布点云信息，如果你使用的是其他公司的RGBD相机，可用参考官方的深度相机配置方法：
sensors:
- sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
image_topic: /head_mount_kinect/depth_registered/image_raw
queue_size: 5
near_clipping_plane_distance: 0.3
far_clipping_plane_distance: 5.0
shadow_threshold: 0.2
padding_scale: 4.0
padding_offset: 0.03
filtered_cloud_topic: filtered_cloud2.填写launch文件夹下的redwall_arm_moveit_sensor_manager.launch.xml,在launch目录下，有两个与sensor相关的launch文件,一个是sensor_manager.launch.xml,该文件是moveit生成配置文件后自动生成好的。另一个是XXX_moveit_sensor_manager.launch.xml文件，这也是自动生成好的，只不过是个空的launch文件，需要填写以下内容。
第一个参数是你想要将八叉树地图发布在那个坐标系，我的发布在camera_link，其余两个参数就是和八叉树地图的分辨率有关了，据说这个分辨率太大，可能都是地图无法加载显示，反正我没有遇到这个问题。最后一个参数加载的就是上面配置的sensors_3d.yaml了。
3.由于现在还是在疫情阶段，还没有进行手眼标定，无法确定相机坐标系和世界坐标系的坐标变换，所以简单修改realsense包中的demo_pointcloud.launch文件，将RViz注释掉，添加一个camera_link到base_link的静态坐标变换。
4.接下来就是启动节点，连上深度相机，查看效果了：
# 启动相机节点发布点云数据
$ roslaunch realsense2_camera demo_pointcloud.launch
# 启动moveit
$ roslaunch redwall_arm_moveit_config demo.launch
# 查看坐标变换
$ rosrun rqt_tf_tree rqt_tf_tree 
