objectdetection_tf_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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()


checkerboard_detector
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Tue Jul 2 2019 19:40:33