00001
00002 import rospy
00003 import roslib
00004 roslib.load_manifest("posedetection_msgs")
00005 roslib.load_manifest("dynamic_tf_publisher")
00006 from posedetection_msgs.msg import ObjectDetection
00007 from geometry_msgs.msg import Transform
00008 from geometry_msgs.msg import PoseStamped
00009 from dynamic_tf_publisher.srv import *
00010 import tf
00011
00012 class ObjectDetectionTfPublisher():
00013 def __init__(self):
00014
00015 if rospy.get_param('~use_simple_tf', False):
00016 self.br = tf.TransformBroadcaster()
00017 self.subscriber = rospy.Subscriber("ObjectDetection", ObjectDetection, self.simple_callback);
00018 else:
00019 self.init_object_messages()
00020 self.frame_id = rospy.get_param("~frame_id", "object")
00021 self.subscriber = rospy.Subscriber("ObjectDetection", ObjectDetection, self.callback);
00022
00023 def init_object_messages(self):
00024 if rospy.has_param('~checker_board_params/position_x'):
00025 position_x = rospy.get_param('~checker_board_params/position_x', 0)
00026 position_y = rospy.get_param('~checker_board_params/position_y', 0)
00027 position_z = rospy.get_param('~checker_board_params/position_z', 0)
00028 orientation_x = rospy.get_param('~checker_board_params/orientation_x', 0)
00029 orientation_y = rospy.get_param('~checker_board_params/orientation_y', 0)
00030 orientation_z = rospy.get_param('~checker_board_params/orientation_z', 0)
00031 orientation_w = rospy.get_param('~checker_board_params/orientation_w', 0)
00032 rospy.loginfo("objectdetection_tf_publisher load the calibration params :")
00033 rospy.loginfo(" pos : %f %f %f orientation %f %f %f %f", position_x, position_y, position_z, orientation_x, orientation_y, orientation_z, orientation_w)
00034
00035 self.send_dynamic_tf_request(position_x, position_y, position_z,
00036 orientation_x,orientation_y,orientation_z,orientation_w,
00037 rospy.get_param('~checker_board_params/header_frame'),
00038 self.frame_id
00039 )
00040 else:
00041 rospy.loginfo("No parameters (~checker_board_params) was found for object detection")
00042
00043 def simple_callback(self, msg):
00044 if msg.objects:
00045 for detected_object in msg.objects:
00046 self.br.sendTransform((detected_object.pose.position.x, detected_object.pose.position.y, detected_object.pose.position.z),
00047 (detected_object.pose.orientation.x, detected_object.pose.orientation.y, detected_object.pose.orientation.z, detected_object.pose.orientation.w),
00048 msg.header.stamp,
00049 detected_object.type,
00050 msg.header.frame_id)
00051
00052 def callback(self, msg):
00053 if msg.objects:
00054 for detected_object in msg.objects:
00055 self.send_dynamic_tf_request(
00056 detected_object.pose.position.x,
00057 detected_object.pose.position.y,
00058 detected_object.pose.position.z,
00059 detected_object.pose.orientation.x,
00060 detected_object.pose.orientation.y,
00061 detected_object.pose.orientation.z,
00062 detected_object.pose.orientation.w,
00063 msg.header.frame_id,
00064 self.frame_id
00065 )
00066
00067 def send_dynamic_tf_request(self, pos_x, pos_y, pos_z, ori_x, ori_y, ori_z, ori_w, parent_frame_id, child_frame_id):
00068 transform = Transform()
00069 transform.translation.x = pos_x
00070 transform.translation.y = pos_y
00071 transform.translation.z = pos_z
00072 transform.rotation.x = ori_x
00073 transform.rotation.y = ori_y
00074 transform.rotation.z = ori_z
00075 transform.rotation.w = ori_w
00076
00077 set_tf_request = SetDynamicTFRequest()
00078 set_tf_request.freq = 10
00079 set_tf_request.cur_tf.header.stamp = rospy.get_rostime()
00080 set_tf_request.cur_tf.header.frame_id = parent_frame_id
00081 set_tf_request.cur_tf.child_frame_id = child_frame_id
00082 set_tf_request.cur_tf.transform = transform
00083
00084 rospy.wait_for_service('/set_dynamic_tf')
00085 set_dynamic_tf = rospy.ServiceProxy('/set_dynamic_tf', SetDynamicTF)
00086 try:
00087 res = set_dynamic_tf(set_tf_request)
00088 except rospy.ServiceException as exc:
00089 print("Service did not process request: " + str(exc))
00090
00091 if __name__== '__main__':
00092 rospy.init_node('objectdetection_tf_publisher', anonymous=True)
00093 object_detection_tf_publisher = ObjectDetectionTfPublisher()
00094 rospy.spin()