Python tf2_ros.TransformBroadcaster方法代码示例

# 需要导入模块: import tf2_ros [as 别名]
# 或者: from tf2_ros import TransformBroadcaster [as 别名]
def publish_pose_as_transform(pose, reference_frame, name, seconds=1):
    """
    Publish a pose as a transform so that it is visualised in rviz.
    pose                -> A geometry_msgs.msg/Pose to be converted into a transform and published
    reference_frame     -> A string defining the reference_frame of the pose
    name                -> A string defining the child frame of the transform
    seconds             -> An int32 that defines the duration the transform will be broadcast for
    """

    # Create a broadcast node and a stamped transform to broadcast
    br = tf2_ros.TransformBroadcaster()
    t = gmsg.TransformStamped()

    # Prepare broadcast message
    t.header.frame_id = reference_frame
    t.child_frame_id = name

    # Copy in pose values to transform
    t.transform.translation = pose.position
    t.transform.rotation = pose.orientation

    # Call the publish_stamped_transform function
    publish_stamped_transform(t, seconds) 

# 需要导入模块: import tf2_ros [as 别名]
# 或者: from tf2_ros import TransformBroadcaster [as 别名]
def publish_stamped_transform(stamped_transform, seconds=1):
    """
    Publish a stamped transform for debugging purposes.
        stamped_transform       -> A geometry_msgs/TransformStamped to be published
        seconds                 -> An int32 that defines the duration the transform will be broadcast for
    """
    # Create broadcast node
    br = tf2_ros.TransformBroadcaster()

    # Publish once first.
    stamped_transform.header.stamp = rospy.Time.now()
    br.sendTransform(stamped_transform)

    # Publish transform for set time.
    i = 0
    iterations = seconds/0.05
    while not rospy.is_shutdown() and i < iterations:
        stamped_transform.header.stamp = rospy.Time.now()
        br.sendTransform(stamped_transform)
        rospy.sleep(0.05)
        i += 1 

# 需要导入模块: import tf2_ros [as 别名]
# 或者: from tf2_ros import TransformBroadcaster [as 别名]
def __init__(self):
        """
        Constructor
        """
        if MISSING_IMPORT is not None:
            raise pybullet.error(MISSING_IMPORT)

        self.spin_thread = None
        self._wrapper_termination = False
        self.image_bridge = CvBridge()
        self.front_info_msg = dict()
        self.bottom_info_msg = dict()
        self.depth_info_msg = dict()
        self.roslauncher = None
        self.transform_broadcaster = tf2_ros.TransformBroadcaster()
        atexit.register(self.stopWrapper) 

# 需要导入模块: import tf2_ros [as 别名]
# 或者: from tf2_ros import TransformBroadcaster [as 别名]
def main(_):
    """ Main function, starts predictor.
    """
    print('main')
    rospy.init_node('costar_hyper_prediction')
    predictor = CostarHyperPosePredictor()

    broadcaster = tf2.TransformBroadcaster()
    transform_name = 'predicted_goal_' + predictor.ee_frame
    rospy.loginfo('costar_hyper_prediction transform predictions will be broadcast as: ' + str(transform_name))
    update_rate = 0.25
    rate = rospy.Rate(update_rate)
    progbar = tqdm()

    while not rospy.is_shutdown():
        rate.sleep()
        start_time = time.clock()
        prediction_xyz_qxyzw, prediction_input_data_time = predictor()
        tick_time = time.clock()

        b_to_e = TransformStamped()
        b_to_e.header.stamp = prediction_input_data_time
        b_to_e.header.frame_id = predictor.base_link
        b_to_e.child_frame_id = transform_name
        b_to_e.transform.translation.x = prediction_xyz_qxyzw[0]
        b_to_e.transform.translation.y = prediction_xyz_qxyzw[1]
        b_to_e.transform.translation.z = prediction_xyz_qxyzw[2]
        b_to_e.transform.rotation.x = prediction_xyz_qxyzw[3]
        b_to_e.transform.rotation.y = prediction_xyz_qxyzw[4]
        b_to_e.transform.rotation.z = prediction_xyz_qxyzw[5]
        b_to_e.transform.rotation.w = prediction_xyz_qxyzw[6]
        broadcaster.sendTransform(b_to_e)
        update_time = time.clock()

        # figure out where the time has gone
        time_str = ('Total tick + log time: {:04} sec, '
                    'Robot Prediction: {:04} sec, '
                    'Sending Results: {:04} sec'.format(update_time - start_time, tick_time - start_time, update_time - tick_time))
        verify_update_rate(update_time_remaining=rate.remaining(), update_rate=update_rate, info=time_str)
        progbar.update() 

# 需要导入模块: import tf2_ros [as 别名]
# 或者: from tf2_ros import TransformBroadcaster [as 别名]
def __debug_loop(self):
        """
        Debug loop that will run in a separate thread.
        (tfBuffer should be threadsafe)
        """
        broadcaster = tf2_ros.TransformBroadcaster()
        rviz_marker_pub = rospy.Publisher('/visualization_marker', Marker,
                                          queue_size=1000)
        rate = rospy.Rate(5)
        while not self.__debug_stop_event.is_set() and not rospy.is_shutdown():
            if self.__debug_current_ws is None:
                print "Could not publish debug tf, no workspace set."
                rate.sleep()
                continue

            try:
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        "base_link", self.__debug_current_ws.name,
                        rospy.Time(0))
                )
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        self.__debug_current_ws.name, "object_base",
                        rospy.Time(0))
                )
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        "object_base", "tool_link_target", rospy.Time(0))
                )
            except tf2_ros.LookupException as e:
                print "Could not publish debug tf: ", e

            for i in range(4):  # Iterate over the 4 markers defining the workspace
                msg = Marker()
                msg.header.frame_id = "base_link"
                msg.id = i
                msg.type = 2  # It correspond to a sphere which will be drawn

                msg.pose.position.x = self.__debug_current_ws.points[i][0]
                msg.pose.position.y = self.__debug_current_ws.points[i][1]
                msg.pose.position.z = self.__debug_current_ws.points[i][2]

                msg.scale.x = 0.005
                msg.scale.y = 0.005
                msg.scale.z = 0.005

                msg.color.r = 1.0 if i == 0 or i == 3 else 0.0
                msg.color.g = 1.0 if i == 1 or i == 3 else 0.0
                msg.color.b = 1.0 if i == 2 or i == 3 else 0.0
                msg.color.a = 1.0

                rviz_marker_pub.publish(msg)

        rate.sleep()