Add service for printing Baxter pose and angles

src_python/lib_baxter.py

@@ -23,7 +23,7 @@ def getJointAngles(self):
         return angles
 
     def getGripperPose(self, flag_return_euler=True):
-        tmp = self.limb_kinematics.forward_position_kinematics()
+        tmp = self.limb_kinematics.forward_position_kinematics().tolist()
         # fk returned type: [x, y, z, quat_i, quat_j, quat_k, quat_w]
         position = tmp[0:3]
         quaternion = tmp[3:]

srv/PrintBaxterGripperPose.srv

@@ -1,4 +1,7 @@
 ---
-# bool success
+bool success
+float32[] position
+float32[] quaternion
+float32[] euler
 # geometry_msgs/Pose pose
-string res_to_print
+# string res_to_print

srv/PrintBaxterJointAngles.srv

@@ -1,4 +1,4 @@
 ---
-# bool success
-# float32[] joint_angles
-string res_to_print
+bool success
+float32[] joint_angles
+# string res_to_print

test_ros/my_baxter_services_provider.py

@@ -4,6 +4,7 @@
 ''' Command line usage:
     rosservice list |grep my
     rosservice call my/PrintBaxterGripperPose
+    rosservice call my/PrintBaxterJointAngles
 '''
 # Standard
 import numpy as np
@@ -21,39 +22,50 @@
 from scan3d_by_baxter.srv import * # import the above's responses types
 
 DEBUG = False
-NUM_DIGITS_TO_PRINT = 4
+NUM_DECIMALS_TO_PRINT = 4 
 
 def DEBUG_PROGRAM():
     if DEBUG:
         res=callService("my/PrintBaxterGripperPose", PrintBaxterGripperPose)
         res=callService("my/PrintBaxterJointAngles", PrintBaxterJointAngles)
 
 def list2str(vals):
-    str_format="{:."+str(NUM_DIGITS_TO_PRINT)+"f} " #{:.4f}
+    str_format="{:."+str(NUM_DECIMALS_TO_PRINT)+"f} " #{:.4f}
     str_val = [str_format.format(val) for val in vals]
     str_val = ", ".join(str_val)
     return str_val
 
+def truncateFloat(val, num_decimals = 4): # (0.123456789, 4) -> 0.1234
+    return val
+    # -- These function is commented out.
+    # -- The reason is, the float number to ros service becomes inaccurate. This func will 
+    # -- return something like this: -1.7077000141143799, even if I set it 4 decimals. 
+    # ratio_ = 10**num_decimals
+    # truncate_ = lambda x: round(x*ratio_)*1.0/ratio_
+    # if type(val)==list:
+    #     return [truncate_(x) for x in val]
+    # else:
+    #     return truncate_(val)
+
 def callback_PrintBaxterGripperPose(req):
     position, quaternion = myBaxter.getGripperPose(flag_return_euler=False)
     position, euler = myBaxter.getGripperPose(flag_return_euler=True)
-    # -- Print
-    rospy.loginfo("\nPrint Baxter gripper pose:")
-    rospy.loginfo("-- x,y,z = " + list2str(position))
-    rospy.loginfo("-- quaternion xyzw = " + list2str(quaternion))
-    rospy.loginfo("-- euler xyz = " + list2str(euler))
-    # -- Output
-    # pose = Pose(Point(position[0],position[1],position[2]),
-    #     Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3]))
-    # return PrintBaxterGripperPoseResponse(True, pose)
-    return PrintBaxterGripperPoseResponse()
+    if DEBUG:
+        rospy.loginfo("")
+        rospy.loginfo("Print Baxter gripper pose:")
+        rospy.loginfo("-- x,y,z = " + list2str(position))
+        rospy.loginfo("-- quaternion xyzw = " + list2str(quaternion))
+        rospy.loginfo("-- euler xyz = " + list2str(euler))
+    return PrintBaxterGripperPoseResponse(True,
+        truncateFloat(position), truncateFloat(quaternion), truncateFloat(euler))
 
 def callback_PrintBaxterJointAngles(req):
     angles = myBaxter.getJointAngles()
-    rospy.loginfo("\nPrint Baxter joint angles:")
-    rospy.loginfo("   " + list2str(angles))
-    # return PrintBaxterJointAnglesResponse(True, angles)
-    return PrintBaxterJointAnglesResponse()
+    if DEBUG:
+        rospy.loginfo("")
+        rospy.loginfo("Print Baxter joint angles:")
+        rospy.loginfo("   " + list2str(angles))
+    return PrintBaxterJointAnglesResponse(True, truncateFloat(angles))
 
 
 if __name__ == "__main__":
@@ -68,5 +80,6 @@ def callback_PrintBaxterJointAngles(req):
     s2 = rospy.Service('my/PrintBaxterJointAngles', PrintBaxterJointAngles, callback_PrintBaxterJointAngles)
 
     # Spin
+    rospy.loginfo("\n\nmy_baxter_services_provider starts!\n")
     DEBUG_PROGRAM()
     rospy.spin()