繼續昨天的內容總結，之前已經成功將識別到的物體位置發佈出來作爲一個全局變量。這一篇則是講最終的問題，雖然前面的步驟都做完了，而且最後這步沒什麼實質性的編程，但是一直機械臂一直有問題，且運行錯誤或者到達不了預設的位置，本篇即以此爲基礎進行總結，分析最終不能成功可能的原因，並給出一個合理的開發調試建議。
首先慣例先給出幾篇參考網址
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.要用翻牆 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) -----> base_link: (%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f) at time %.2f",
real_pose.pose.position.x, real_pose.pose.position.y, real_pose.pose.position.z, real_pose.pose.orientation.x, real_pose.pose.orientation.y, real_pose.pose.orientation.z, real_pose.pose.orientation.w,
transed_pose.pose.position.x, transed_pose.pose.position.y, transed_pose.pose.position.z, transed_pose.pose.orientation.x, transed_pose.pose.orientation.y, transed_pose.pose.orientation.z, transed_pose.pose.orientation.w, transed_pose.header.stamp.toSec());
}
catch(tf::TransformException& ex){
ROS_ERROR("Received an exception trying to transform a point from \"camera_rgb_optical_frame\" to \"base_link\": %s", ex.what());
}
}
int main(int argc, char** argv){
ros::init(argc, argv, "tf_listener_Posetransform");
ros::NodeHandle n;
tf::TransformListener listener(ros::Duration(10));
//we'll transform a point once every second
ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPose, boost::ref(listener)));
ros::Publisher my_publisher_realPose = n.advertise<geometry_msgs::PoseStamped>("real_pose", 1000);
ros::Publisher my_publisher_transedPose = n.advertise<geometry_msgs::PoseStamped>("transed_pose", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
my_publisher_transedPose.publish(transed_pose);
my_publisher_realPose.publish(real_pose);
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
return 0;
}
而且可以看到我也在這裏試驗是不是座標系的名稱不對導致位置過不去（點的位置就是識別到的物體的位置），最終選定了camera_optical_rgb_frame，然後點也到了對應的位置。
所以確定了固定點的座標位置並不在罐子實際位置是因爲real_pose.header.frame_id = "camera_rgb_optical_frame"; 而非camera_link 而且從這個圖也可以確定另一個事實就是轉換過程沒有錯誤（從rviz裏面可看到固定一個點的轉換後其點在同一個位置，轉換後的點transed_pose和real_pose重合），也就是之前所說 的可能的原因的3（1）（2）確定不是。當然隨着相機的開啓位置可能會跳躍到不正常的位置，但是基本上已經很少出現。
運行tf_listener.cpp(上一篇已經有程序這裏不在添加)
#rosrun robot_setup_tf tf_listener
可以看到轉換後的點也發佈出去（話題爲transformPose）
這裏要注意的是transed_pose和real_pose是Oneposetransform.cpp幫助我們調試的固定位置，而transformPose則是tf_listener實際所用的tf轉換過程的總程序。如果三點重合則證明標定過程準確無誤。運行ork檢測程序：
#rosrun object_recognition_core detection -c `rospack find object_recognition_linemod`/conf/detection.ros.ork
這樣就將tf轉換過程中的錯誤排查完畢，也全部修改，正確的結果也已經出現。
接着就是解決可能的原因1，位置過不去。把轉換後的一個值直接送到最終的python控制程序依然是：
#rosrun ur10_rg2_moveit_config new_moveit_controller.py
在所設計到達位置不能達到
將轉換後的值與預設的固定值比較發現在position中z值偏小或者錯誤，可能原因就是之前所提到的1的震動所致，所以添加偏移量
print "============ Press `Enter` to plan pepsi0"
raw_input()
pose_goal.orientation.x = -0.702487814174
pose_goal.orientation.y = -0.01293060105
pose_goal.orientation.z = 0.0153163491798
pose_goal.orientation.w = 0.711413438122
pose_goal.position.x = transed_pose.x+0.05
# 0.01 0.05 0.0119
pose_goal.position.y = transed_pose.y-0.05
pose_goal.position.z = transed_pose.z+0.438
group.set_pose_target(pose_goal)
plan00=group.plan()
print "============ Press `Enter` to go pepsi0"
raw_input()
plan = group.go(wait=True)
這裏要提的是transed_pose已經是全局變量，在實驗過程中我是用固定值（當然tf_listener裏面的變量的值就是固定值，只是直接寫出來避免不必要的錯誤），一步步修改+_的值作爲偏移量，以使機械手抓到可樂罐。
這樣就確定了不是linemod算法的限制導致，抓取任務完成。所以機械手臂python控制程序是：
def go_to_pose_goal(self):
group = self.group
group.set_max_velocity_scaling_factor(0.06)
group.set_max_acceleration_scaling_factor(0.01)
pose_goal = geometry_msgs.msg.Pose()
global transed_pose
global transed_orientation
if(transed_pose!=(0,0,0)and transed_orientation!=(0,0,0,0)):
print "===========TransPose-completed============="
rospy.loginfo(rospy.get_caller_id() + 'heard transed_pose %s', transed_pose)
rospy.loginfo(rospy.get_caller_id() + 'heard transed_orientation %s', transed_orientation)
else:
print "===========nothing detected!!!"
print "~~~~~~~~~~~~~~start to compile UR10+RG2~~~~~~~~~~~~~~~~~~~~~~~"
print "============ Press `Enter` to rg2-0"
raw_input()
# rospy.sleep(0.5)
self.request.target_width.data=100.0
resp1=self.val(self.request)
# print "============ Press `Enter` to plan button0"
# raw_input()
# # rospy.sleep(0.5)
# q = tf.transformations.quaternion_from_euler(math.radians(-90.0), 0.0, 0.0)
# pose_goal.orientation.x = q[0]
# pose_goal.orientation.y = q[1]
# pose_goal.orientation.z = q[2]
# pose_goal.orientation.w = q[3]
# pose_goal.position.x = 0.6
# pose_goal.position.y = -0.65
# pose_goal.position.z = 1.40
print "===========this is a test====================="
# group.set_pose_target(pose_goal)
# plan00=group.plan()
# print "============ Press `Enter` to go button0"
# raw_input()
# # rospy.sleep(0.5)
# plan = group.go(wait=True)
print "============ Press `Enter` to plan pepsi0"
raw_input()
pose_goal.orientation.x = -0.702487814174
pose_goal.orientation.y = -0.01293060105
pose_goal.orientation.z = 0.0153163491798
pose_goal.orientation.w = 0.711413438122
pose_goal.position.x = transed_pose.x+0.05
# 0.01 0.05 0.0119
pose_goal.position.y = transed_pose.y-0.05
pose_goal.position.z = transed_pose.z+0.438
group.set_pose_target(pose_goal)
plan00=group.plan()
print "============ Press `Enter` to go pepsi0"
raw_input()
plan = group.go(wait=True)
print "============ Press `Enter` to plan pepsi1"
raw_input()
pose_goal.orientation.x = -0.702487814174
pose_goal.orientation.y = -0.01293060105
pose_goal.orientation.z = 0.0153163491798
pose_goal.orientation.w = 0.711413438122
pose_goal.position.x = transed_pose.x+0.13
# 0.01 0.05 0.0119
pose_goal.position.y = transed_pose.y-0.03
pose_goal.position.z = transed_pose.z+0.399
group.set_pose_target(pose_goal)
plan00=group.plan()
print "============ Press `Enter` to go pepsi1"
raw_input()
plan = group.go(wait=True)
# print "============ Press `Enter` to plan pepsi1"
# raw_input()
# pose_goal.orientation.x = -0.536990182186
# pose_goal.orientation.y = -0.458113016546
# pose_goal.orientation.z = 0.469381574743
# pose_goal.orientation.w = 0.530523275265
# pose_goal.position.x = 1.05278668153
# # 0.01 0.05 0.01
# pose_goal.position.y = 0.0549727717948
# pose_goal.position.z = 0.281837835693
# group.set_pose_target(pose_goal)
# plan01=group.plan()
# print "============ Press `Enter` to go pepsi1"
# raw_input()
# plan = group.go(wait=True)
# print "============ Press `Enter` to plan1"
# # pose_goal.orientation.x = -0.591549509239
# # pose_goal.orientation.y = 0.423087935108
# # pose_goal.orientation.z = 0.369947290233
# # pose_goal.orientation.w = 0.578104471298
# # pose_goal.position.x = 1.17165389661
# # pose_goal.position.y = 0.128525502785
# # pose_goal.position.z = 0.341104864008
# pose_goal.orientation.x = -0.697785749856
# pose_goal.orientation.y = 0.130047030626
# pose_goal.orientation.z = 0.06811187155
# pose_goal.orientation.w = 0.701101697385
# pose_goal.position.x = 1.30709340958
# pose_goal.position.y = 0.121683281188
# pose_goal.position.z = 0.194759575987
# group.set_pose_target(pose_goal)
# plan1=group.plan()
# print "============ Press `Enter` to go1"
# raw_input()
# plan = group.go(wait=True)
# print "============ Press `Enter` to plan2"
# raw_input()
# # pose_goal.orientation.x = -0.691356864871
# # pose_goal.orientation.y = 0.17258390508
# # pose_goal.orientation.z = 0.113781808587
# # pose_goal.orientation.w = 0.692310754747
# # pose_goal.position.x = 1.18815572137
# # pose_goal.position.y = 0.128383418654
# # pose_goal.position.z = 0.178566985603
# pose_goal.orientation.x = -0.701759134111
# pose_goal.orientation.y = 0.0816647083548
# pose_goal.orientation.z = 0.0196895921354
# pose_goal.orientation.w = 0.707444211979
# pose_goal.position.x = 1.46766476156
# pose_goal.position.y = 0.118317513539
# pose_goal.position.z = 0.194995276671
# group.set_pose_target(pose_goal)
# plan2=group.plan()
# print "============ Press `Enter` to go2"
# raw_input()
# plan = group.go(wait=True)
print "============ Press `Enter` to rg2"
raw_input()
# rospy.sleep(0.5)
self.request.target_width.data=73.0
resp1=self.val(self.request)
print "============ Press `Enter` to plan pepsi2"
raw_input()
pose_goal.orientation.x = -0.702080879273
pose_goal.orientation.y = 0.0230291512084
pose_goal.orientation.z = -0.0353026755348
pose_goal.orientation.w = 0.710848660584
pose_goal.position.x = 0.979028034434
pose_goal.position.y = 0.131569260388
pose_goal.position.z = 1.10972751302
group.set_pose_target(pose_goal)
plan3=group.plan()
print "============ Press `Enter` to go pepsi2"
raw_input()
plan = group.go(wait=True)
print "============ Press `Enter` to plan Tochair0"
raw_input()
pose_goal.orientation.x = -0.484546258297
pose_goal.orientation.y = -0.508480168279
pose_goal.orientation.z = 0.525418382276
pose_goal.orientation.w = 0.480206586381
pose_goal.position.x = 0.999753371747
pose_goal.position.y = 0.534242702225+0.01
pose_goal.position.z = 0.599301582776-0.01
group.set_pose_target(pose_goal)
plan3=group.plan()
print "============ Press `Enter` to go Tochair0"
raw_input()
plan = group.go(wait=True)
print "============ Press `Enter` to Tochair1"
raw_input()
# rospy.sleep(0.5)
self.request.target_width.data=100.0
resp1=self.val(self.request)
print "============ Press `Enter` to plan Tochair2"
raw_input()
pose_goal.orientation.x = -0.484546258297
pose_goal.orientation.y = -0.508480168279
pose_goal.orientation.z = 0.525418382276
pose_goal.orientation.w = 0.480206586381
pose_goal.position.x = 0.999753371747
pose_goal.position.y = 0.534242702225-0.1
pose_goal.position.z = 0.599301582776-0.015
group.set_pose_target(pose_goal)
plan3=group.plan()
print "============ Press `Enter` to go Tochair2"
raw_input()
plan = group.go(wait=True)
print "============ Press `Enter` to plan Tochair3"
raw_input()
pose_goal.orientation.x = 6.79752193197e-05
pose_goal.orientation.y = 6.79780791568e-05
pose_goal.orientation.z = 0.707089856963
pose_goal.orientation.w = 0.707123698471
pose_goal.position.x = 0.125088661163
pose_goal.position.y = 0.451141001514
pose_goal.position.z = 1.90037518571
group.set_pose_target(pose_goal)
plan3=group.plan()
print "============ Press `Enter` to go Tochair3"
raw_input()
plan = group.go(wait=True)
# print "============ Press `Enter` to plan button1"
# raw_input()
# # rospy.sleep(0.5)
# q = tf.transformations.quaternion_from_euler(math.radians(-90.0), 0.0, 0.0)
# pose_goal.orientation.x = q[0]
# pose_goal.orientation.y = q[1]
# pose_goal.orientation.z = q[2]
# pose_goal.orientation.w = q[3]
# pose_goal.position.x = 0.65
# pose_goal.position.y = -0.65
# pose_goal.position.z = 1.40
# group.set_pose_target(pose_goal)
# plan00=group.plan()
# print "============ Press `Enter` to go button1"
# raw_input()
# # rospy.sleep(0.5)
# plan = group.go(wait=True)
# print "============ Press `Enter` to plan button2"
# raw_input()
# # rospy.sleep(0.5)
# q = tf.transformations.quaternion_from_euler(math.radians(-90.0), 0.0, 0.0)
# pose_goal.orientation.x = q[0]
# pose_goal.orientation.y = q[1]
# pose_goal.orientation.z = q[2]
# pose_goal.orientation.w = q[3]
# pose_goal.position.x = 0.6
# pose_goal.position.y = -0.65
# pose_goal.position.z = 1.40
# group.set_pose_target(pose_goal)
# plan00=group.plan()
# print "============ Press `Enter` to go button2"
# raw_input()
# # rospy.sleep(0.5)
# plan = group.go(wait=True)
group.stop()
group.clear_pose_targets()
current_pose = self.group.get_current_pose().pose
print self.group.get_current_pose()
return all_close(pose_goal, current_pose, 0.01)
關於測試視頻：
https://pan.baidu.com/s/1H4Ul8tlpuHkyjuyL1qttjQ
當然理應是放在youtube上面的，以後再傳吧。
總的程序的運行步驟：
執行步驟：
#roslaunch realsense_camera zr300_nodelet_rgbd.launch
#roslaunch ur_control ur10_control.launch robot_ip:=192.168.2.24
#roslaunch easy_handeye static_transform_publisher.launch
#rosrun object_recognition_core detection -c `rospack find object_recognition_linemod`/conf/detection.ros.ork
(#rosrun rqt_reconfigure rqt_reconfigure)
#roslaunch ur10_rg2_moveit_config demo.launch
#rosrun robot_setup_tf tf_broadcaster
#rosrun robot_setup_tf tf_listener
(#rosrun ur10_rg2_moveit_config my_pose_print.py打印當前機械手位置)
(#rosrun robot_setup_tf tf_listener_OnePosetransform轉換實驗一個點是否重合（可以測試在rviz裏面顯示）)
（#rosrun ur10_rg2_moveit_config listener.py打印識別到的物體位置）
機器人控制
#rosrun ur10_rg2_moveit_config moveit_controller.py
//加限制
#rosrun ur10_rg2_moveit_config new_moveit_controller.py
完成所有內容的總結，前後一共三個月，從零做到現在。之後再把這些程序整合一下，就可以放在算法的創新上，以便之後的發論文，繼續努力↖(^ω^)↗