博客

做完标定后，测试避障功能，都是坑

typedef pcl::PointXYZRGB PointT;
pcl::transformPointCloud(*cloud_in,*cloud,cam2rb);
pcl::copyPointCloud(*cloud_in,*cloud);
pcl::io::savePCDFileASCII ("cloud1_trans.pcd", *cloud); //pcl::io::savePCDFileBinary("cloud1_trans.pcd", *cloud); 

若以pcl::PointXYZRGB读取pcd文件，copy一份然后保存到硬盘，用pcl_viewer看不到保存的点云

而后以pcl::PointXYZ读取，便可以看到。之前遇到到这个问题，如果pcd文件不含有rgb信息但强行以pcl::PointXYZRGB读取会自动赋值rgb为黑色（0， 0， 0），而pcl_viewer背景为黑色，所以看不到任何点云。but这次我读取的是带有rgb信息的pcd啊…

这是小插曲，后面开始踩坑

坑一

方法：moveit自带的MotionPlanning可以直接import file

试了下不能直接导入.pcd文件，但是可以直接导入.ply文件，所以直接运用

pcl::transformPointCloud(*cloud_in,*cloud,cam2rb);

得到基于机械臂base的点云，然后直接通过终端命令

pcl_pcd2ply xxx.pcd  xxx.ply

运行打开rviz

roslaunch ur3_moveit_config moveit_rviz.launch config:=true

import网上的standford bunny，效果如图：



import刚才生成的xxx.ply文件,报错如下：

eigen: too many iterations in Jacobi transform.
rviz: /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/include/OgreAxisAlignedBox.h:252：void Ogre::AxisAlignedBox::setExtents(const Ogre::Vector3&, const Ogre::Vector3&): 假设 ‘(min.x <= max.x && min.y <= max.y && min.z <= max.z) && "The minimum corner of the box must be less than or equal to maximum corner"’ 失败。

未解决。

坑二

方法：直接发布octomap给PlanningScene

定义PlanningScene的发布器

ros::Publisher octomap_pub = n.advertise<moveit_msgs::PlanningScene>("/planning_scene", 1);

获取octomap数据

octomap::OcTree* octree = new octomap::OcTree(argv[1]);//命令行参数读入xxx.bt文件
static octomap_msgs::Octomap octomap;
octomap_msgs::binaryMapToMsg(*octree, octomap);//转换成消息格式

定义planningscene的消息

moveit_msgs::PlanningScene planning_scene;
planning_scene.world.octomap.header.frame_id = "odom_combined";
planning_scene.world.octomap.header.stamp = ros::Time::now();
planning_scene.world.octomap.octomap.header.frame_id = "odom_combined";
planning_scene.world.octomap.octomap.header.stamp = ros::Time::now();
planning_scene.world.octomap.octomap.binary = true;
planning_scene.world.octomap.octomap.id = "OcTree";
planning_scene.world.octomap.octomap.data = octomap.data;

发布PlanningScene消息

octomap_pub.publish(planning_scene);

由于rviz一直没有显示出octomap数据，添加如下语句，验证rviz有在订阅消息

while (octomap_pub.getNumSubscribers() < 1)
{
    ros::WallDuration sleep_t(0.5);
    sleep_t.sleep();
}
ROS_INFO("more than one subscriber, start publishing msgs on and on...");

但是rviz什么场景都显示不出来，通过

rostopic echo /planning_scene

发现主要问题是消息的seq为0



网上说主要是没有订阅者。。。

未解决。

最后尝试了config的方法，终于可行了

建立sensor_moveit_config包（我放在了universial_robot文件夹内，新建一个package专门放sensor配置，方便ur3/5/10调用）

catkin_create_pkg sensor_moveit_config roscpp rospy std_msgs moveit_ros_move_group moveit_ros_visualization

在sensor_moveit_config包内新建config和launch文件夹

config文件夹内新建sensors_rgbd.yaml，填入如下内容

sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
  point_cloud_topic: /zed_trans
  max_range: 10
  padding_offset: 0.01
  padding_scale: 1.0
  point_subsample: 1
  filtered_cloud_topic: output_cloud
# this last topic is just to debug, having it adds processing time


# sensors:
#  - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
#    image_topic: /head_mount_xtion/depth/image_raw
#    queue_size: 5
#    near_clipping_plane_distance: 0.3
#    far_clipping_plane_distance: 5.0
#    skip_vertical_pixels: 1
#    skip_horizontal_pixels: 1
#    shadow_threshold: 0.2
#    padding_scale: 4.0
#    padding_offset: 0.03
#    filtered_cloud_topic: output_cloud

在launch文件夹内新建kinect2_moveit_sensor_manager.launch.xml和sensor_manager.launch.xml

<launch>
   <rosparam command = "load" file="$(find sensor_moveit_config)/config/sensors_rgbd.yaml" />
</launch>

<launch>

  <!-- This file makes it easy to include the settings for sensor managers -->  

  <!-- Params for the octomap monitor -->
  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
  <param name="octomap_resolution" type="double" value="0.025" />
  <param name="max_range" type="double" value="5.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="kinect2" />
  <include file="$(find sensor_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

编译

catkin_make

在ur3_moveit_config/launch/move_group.launch添加如下程序

<include ns="move_group" file="$(find sensor_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
   <arg name="moveit_sensor_manager" value="kinect2" />
</include>

命令行按顺序调用：

roslaunch ur_gazebo ur3_joint_limited.launch
roslaunch ur3_moveit_config ur3_moveit_planning_execution.launch sim:=true limited:=true
roslaunch ur3_moveit_config moveit_rviz.launch config:=true
rosrun rviz_show rviz_show_octo_from_pcd redbox.pcd 

结果如下：

参考

3D Perception Moveit Docs



ros机器人程序设计Indigo



github——pr2 moveit config

坑二相关


moveit_msgs——PlanningScene



添加各种格式的planningScene



seq = 0的问题



Why does ROS overwrite my sequence number?