in_hand_recognition_manager.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # program for recognition in hand
00004 # depends on the node-structure of icp_registration
00005 # suppose that robot does not move throughout recognizing
00006 
00007 import rospy
00008 
00009 PKG='jsk_pcl_ros'
00010 
00011 import imp
00012 try:
00013     imp.find_module(PKG)
00014 except:
00015     import roslib;roslib.load_manifest(PKG)
00016 
00017 from geometry_msgs.msg import PoseStamped, Pose
00018 from sensor_msgs.msg import PointCloud2
00019 from jsk_recognition_msgs.msg import PointsArray
00020 from tf.transformations import *
00021 import tf
00022 from std_srvs import srv
00023 
00024 teacher_pose_stamped = None
00025 renew_flag = False
00026 def pose_teacher_cb(pose_stamped):
00027     global teacher_pose_stamped
00028     teacher_pose_stamped = pose_stamped
00029     InputPosePub.publish(teacher_pose_stamped)
00030     # pub pose and save pose
00031 def get_mat_from_pose(pose):
00032     return concatenate_matrices(
00033       translation_matrix([pose.position.x, pose.position.y, pose.position.z]),
00034       quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
00035       )
00036 def get_pose_from_mat(mat):
00037     translation = translation_from_matrix(mat)
00038     rotation = quaternion_from_matrix(mat)
00039     pose = Pose()
00040     pose.position.x = translation[0]
00041     pose.position.y = translation[1]
00042     pose.position.z = translation[2]
00043     pose.orientation.x = rotation[0]
00044     pose.orientation.y = rotation[1]
00045     pose.orientation.z = rotation[2]
00046     pose.orientation.w = rotation[3]
00047     return pose
00048     
00049 def pose_diff_cb(pose_stamped):
00050     global teacher_pose_stamped, renew_flag
00051     # add diff and pub
00052     if (not teacher_pose_stamped):
00053         rospy.info ("teacher is empty")
00054         return
00055     # DummyArrayPub.publish(PointsArray()) # register empty clouds to stop recognition
00056     try:
00057         teacher_pose_stamped.header.stamp = rospy.Time(0)
00058         teacher_pose_stamped_recog_frame = listener.transformPose(pose_stamped.header.frame_id, teacher_pose_stamped)
00059     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception), e:
00060         print "tf error: %s" % e
00061         return
00062     teacher_pose_mat = (get_mat_from_pose(teacher_pose_stamped_recog_frame.pose))
00063     diff_pose_mat = (get_mat_from_pose(pose_stamped.pose))
00064     new_pose_mat = concatenate_matrices(diff_pose_mat, teacher_pose_mat)
00065     new_pose_stamped = PoseStamped()
00066     new_pose_stamped.pose = get_pose_from_mat(new_pose_mat)
00067     new_pose_stamped.header = pose_stamped.header
00068     OutputPosePub.publish(new_pose_stamped)
00069     # teachear_pose_stamped = None
00070     if renew_flag:
00071         try:
00072             new_pose_stamped.header.stamp = rospy.Time(0)
00073             new_pose_stamped_for_renew = listener.transformPose(teacher_pose_stamped.header.frame_id, new_pose_stamped)
00074         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception), e:
00075             print "tf error: %s" % e
00076             return
00077         pose_teacher_cb(new_pose_stamped_for_renew)
00078         renew_flag = False
00079 def renew_cb(req):
00080     global renew_flag
00081     renew_flag = True
00082     return srv.EmptyResponse()
00083 if __name__ == "__main__":
00084     rospy.init_node("in_hand_recognition_manager")
00085     InputPosePub = rospy.Publisher("~output/recognition", PoseStamped)
00086     OutputPosePub = rospy.Publisher("~output", PoseStamped)
00087     listener = tf.TransformListener()
00088     DummyArrayPub = rospy.Publisher("~dummy_array", PointsArray)
00089     rospy.Subscriber("~input", PoseStamped, pose_teacher_cb)
00090     rospy.Subscriber("~input/result", PoseStamped, pose_diff_cb)
00091     rospy.Service("~renew", srv.Empty, renew_cb)
00092     rospy.spin()
00093 


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:43