IntelRealSenseZR300+UR10RG2机械手臂+Linemod算法 机械臂抓取可乐罐 系列第六篇

继续昨天的内容总结，之前已经成功将识别到的物体位置发布出来作为一个全局变量。这一篇则是讲最终的问题，虽然前面的步骤都做完了，而且最后这步没什么实质性的编程，但是一直机械臂一直有问题，且运行错误或者到达不了预设的位置，本篇即以此为基础进行总结，分析最终不能成功可能的原因，并给出一个合理的开发调试建议。

首先惯例先给出几篇参考网址

1.这个是之前写的文章下面交流的时候有提到的参考网址，应该也是一个抓取任务或者可能包含更多内容，只是我已经走到了后面几步所以没有时间去参考，这里列出来希望对大家有帮助，以后的话可以慢慢看看是什么用处。

https://github.com/atenpas/gpd/tree/forward

2.关于tf点转换过程用到的callback函数

https://github.com/hu7241/nao_slam_amcl/blob/master/voice_nav_script/gui.py callback

在这个项目的开发过程中，不仅仅是ros机器人相关知识的积累实践，更重要的是和周围的人学习到的开发方法，各种工具。

1.要用fanqiang 2.学知识一定要去官网这样更系统 3.勤用谷歌（英语没问题的话）4.向他人请教  不仅是知识也包括使用的工具 开发调试思路等。

好了开始最后的内容总结。

---

从上一篇继续，在转换为点并把全局变量在最终的python程序订阅 接收到了全局变量以后发现位置并不能到达，并出现了如下的问题：

No motion plan found.No execution attempted.

出现到不了的情况原因很多，这里说明一下可能的原因：

1.位置并不能达到指定的位置，这是由于机械臂的活动范围有限制，所以如果提供的位置太远便不能plan

2.位置不准确或者是错误位置，这可能是由于linemod算法的限制。如果是这个原因的话等排查出来就很麻烦了，需要重新换新的检测方法，换其他的算法，再调试再对接，这样就要花很长很长时间了。

3.位置正确但是转换结果导致位置错误：

这包括

（1）TF_listener转换的问题，因为之前开发程序在转坐标的时候坐标系常常不注意，有时候没有弄懂程序是什么意思的情况下只知道用处便直接使用。事实上最终原因确实是这边的一个程序的坐标系没有弄准确。linemod识别到物体的位置所在的坐标系是camera_rgb_optical_frame而非camera_rgb_frame或者camera_link 这里一定要注意，不然位置错了都不知道是哪步错的。

（2）transform转换错误。这是由于用于调试的相机只是放置在设置位置但并没有固定，所以在机械手臂运动过程中发生的震动会导致之前标记的camera_link和base_link转换关系不准确。这样理应是全部重新标定修改新的转换关系。不过可以通过添加偏移量降低这个不准确转换关系的影响。

首先评估这几个原因，原因3（2）和1的可能性最高。而且linemod算法虽然不完善的地方有很多，但是对于抓取任务是足够的。

针对这个问题进行排查，思路是

1.用一个点来试（也就是之前所提到的OnePosetransform.cpp来协助调试）这样就可以在rviz里面通过订阅话题查看到位置的变化，也有助于排查原因。

2.固定移动机械手臂得到固定值 取其中的orentation ,再用转换后的一个点的position值带入，并且要发布出去在rviz里面查看。

3.重新完成标定过程。这意味着不仅仅是camera_link到base_link的标定还包括TF点的转换，更加深入的理解例程序是什么意思，把坐标系重新改。

 

这里可以看到物体位置并不在实际位置上。调试_实际位置在rg2但测得位置在上面（测得位置与转换位置重合证明转换过程没有错误）

那么之前已经成功转换一个点并发布，OnePosetransform.cpp核心的作用是省掉中间的tf转换过程，直接转相机发布的位置并直接给到最终的py控制程序，并把转换过程中的点发布出去以供rviz里面查看。

(#rosrun robot_setup_tf tf_listener_OnePosetransform转换实验一个点是否重合（可以测试在rviz里面显示）)

#include <ros/ros.h>
// #include <geometry_msgs/PointStamped.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/MarkerArray.h> 

geometry_msgs::PoseStamped real_pose;
geometry_msgs::PoseStamped transed_pose;

void transformPose(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  // geometry_msgs::PointStamped laser_point;
  // geometry_msgs::PoseStamped msg;
  // laser_point.header.frame_id = "camera_link";
  // real_pose.header.frame_id = "camera_link";
  real_pose.header.frame_id = "camera_rgb_optical_frame";
  // real_pose.header.frame_id = "camera_rgb_frame";
  //we'll just use the most recent transform available for our simple example
  // laser_point.header.stamp = ros::Time();
  real_pose.header.stamp = ros::Time();
  real_pose.pose.position.x = -0.0521919690073; 
  real_pose.pose.position.y = 0.0947469994426; 
  real_pose.pose.position.z = 0.928996682167;   
  real_pose.pose.orientation.w = 0.487851679325; 
  real_pose.pose.orientation.x = 0.518789410591; 
  real_pose.pose.orientation.y = -0.516209602356; 
  real_pose.pose.orientation.z = 0.47580036521; 
  //just an arbitrary point in space

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

    ROS_INFO("camera_rgb_frame: (%.2f, %.2f. %.2f, %.2f, %.2f, %.2f, %.2f) ----->