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 dynamic_tf_publisher.srv import *
00009
00010 class ObjectDetectionTfPublisher():
00011 def __init__(self):
00012 self.subscriber = rospy.Subscriber("ObjectDetection", ObjectDetection, self.callback);
00013 self.frame_id = rospy.get_param("~frame_id", "object")
00014 self.init_object_messages()
00015
00016 def init_object_messages(self):
00017 if rospy.has_param('~checker_board_params/position_x'):
00018 position_x = rospy.get_param('~checker_board_params/position_x', 0)
00019 position_y = rospy.get_param('~checker_board_params/position_y', 0)
00020 position_z = rospy.get_param('~checker_board_params/position_z', 0)
00021 orientation_x = rospy.get_param('~checker_board_params/orientation_x', 0)
00022 orientation_y = rospy.get_param('~checker_board_params/orientation_y', 0)
00023 orientation_z = rospy.get_param('~checker_board_params/orientation_z', 0)
00024 orientation_w = rospy.get_param('~checker_board_params/orientation_w', 0)
00025 rospy.loginfo("objectdetection_tf_publisher load the calibration params :")
00026 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)
00027
00028 self.send_dynamic_tf_request(position_x, position_y, position_z,
00029 orientation_x,orientation_y,orientation_z,orientation_w,
00030 rospy.get_param('~checker_board_params/header_frame'),
00031 self.frame_id
00032 )
00033 else:
00034 rospy.loginfo("No parameters (~checker_board_params) was found for object detection")
00035
00036 def callback(self, msg):
00037 if msg.objects:
00038 for detected_object in msg.objects:
00039 self.send_dynamic_tf_request(
00040 detected_object.pose.position.x,
00041 detected_object.pose.position.y,
00042 detected_object.pose.position.z,
00043 detected_object.pose.orientation.x,
00044 detected_object.pose.orientation.y,
00045 detected_object.pose.orientation.z,
00046 detected_object.pose.orientation.w,
00047 msg.header.frame_id,
00048 self.frame_id
00049 )
00050
00051 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):
00052 transform = Transform()
00053 transform.translation.x = pos_x
00054 transform.translation.y = pos_y
00055 transform.translation.z = pos_z
00056 transform.rotation.x = ori_x
00057 transform.rotation.y = ori_y
00058 transform.rotation.z = ori_z
00059 transform.rotation.w = ori_w
00060
00061 set_tf_request = SetDynamicTFRequest()
00062 set_tf_request.freq = 10
00063 set_tf_request.cur_tf.header.stamp = rospy.get_rostime()
00064 set_tf_request.cur_tf.header.frame_id = parent_frame_id
00065 set_tf_request.cur_tf.child_frame_id = child_frame_id
00066 set_tf_request.cur_tf.transform = transform
00067
00068 rospy.wait_for_service('set_dynamic_tf')
00069 set_dynamic_tf = rospy.ServiceProxy('set_dynamic_tf', SetDynamicTF)
00070 try:
00071 res = set_dynamic_tf(set_tf_request)
00072 except rospy.ServiceException as exc:
00073 print("Service did not process request: " + str(exc))
00074
00075 if __name__== '__main__':
00076 rospy.init_node('objectdetection_tf_publisher', anonymous=True)
00077 object_detection_tf_publisher = ObjectDetectionTfPublisher()
00078 rospy.spin()