Python TransformStamped.header方法代码示例

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import header [as 别名]
 def on_result(self, boxes, classes):
     rospy.logdebug("on_result")
     msg = ObjectDetection()
     msg.header = boxes.header
     for box, label, prob in zip(boxes.boxes, classes.label_names, classes.label_proba):
         if not label:
             continue
         pose = Object6DPose()
         pose.pose = box.pose
         pose.reliability = prob
         pose.type = label
         msg.objects.append(pose)
         if self.publish_tf:
             t = TransformStamped()
             t.header = box.header
             t.child_frame_id = label
             t.transform.translation.x = box.pose.position.x
             t.transform.translation.y = box.pose.position.y
             t.transform.translation.z = box.pose.position.z
             t.transform.rotation.x = box.pose.orientation.x
             t.transform.rotation.y = box.pose.orientation.y
             t.transform.rotation.z = box.pose.orientation.z
             t.transform.rotation.w = box.pose.orientation.w
             self.tfb.sendTransform(t)
     self.pub_detect.publish(msg)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import header [as 别名]
def transform_helper(vec3, frame):
    pos = TransformStamped()
    pos.child_frame_id = frame
    pos.header = Header()
    pos.transform = Transform()
    pos.transform.translation = vec3

    return pos

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import header [as 别名]
    def publish_odometry(self):
        quat = tf.transformations.quaternion_from_euler(0, 0, self.yaw)
        quaternion = Quaternion()
        quaternion.x = quat[0]
        quaternion.y = quat[1]
        quaternion.z = quat[2]
        quaternion.w = quat[3]

        if self.last_time is None:
            self.last_time = rospy.Time.now()

        vx = self.vx #(msg->twist.twist.linear.x) *trans_mul_;
        vy = self.vy #msg->twist.twist.linear.y;
        vth = self.vyaw #(msg->twist.twist.angular.z) *rot_mul_;

        current_time = rospy.Time.now()

        dt = (current_time - self.last_time).to_sec()
        delta_x = (vx * math.cos(self.yaw) - vy * math.sin(self.yaw)) * dt
        delta_y = (vx * math.sin(self.yaw) + vy * math.cos(self.yaw)) * dt
        delta_th = vth * dt

        self.x += delta_x
        self.y += delta_y
        self.yaw += delta_th

        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.pose.pose.position.x = self.x
        odom.pose.pose.position.y = self.y
        odom.pose.pose.position.z = 0.0
        odom.pose.pose.orientation = quaternion
        odom.twist.twist.linear.x = vx
        odom.twist.twist.linear.y = vy
        odom.twist.twist.angular.z = vth

        self.odom_pub.publish(odom)

        transform_stamped = TransformStamped()
        transform_stamped.header = odom.header

        transform_stamped.transform.translation.x = self.x
        transform_stamped.transform.translation.y = self.y
        transform_stamped.transform.translation.z = 0.0
        transform_stamped.transform.rotation = quaternion

        #self.tfbr.sendTransform(transform_stamped)
        self.tfbr.sendTransform((self.x, self.y, 0.0),
                                (quat[0], quat[1], quat[2], quat[3]),
                                rospy.Time.now(),
                                "odom",
                                "base_link")

        self.last_time = current_time

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import header [as 别名]
def pose_cb(pose_msg):
    trans = TransformStamped()
    trans.child_frame_id = "handle_orig"
    trans.header = pose_msg.header
    trans.transform.rotation = pose_msg.pose.orientation
    trans.transform.translation.x = pose_msg.pose.position.x
    trans.transform.translation.y = pose_msg.pose.position.y
    trans.transform.translation.z = pose_msg.pose.position.z
    set_tf(10, trans)  # orig

    try:
        transed_pose = listener.transformPose("BODY", pose_msg)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
        print "tf error: %s" % e
        return

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import header [as 别名]
 def publish_filtered_tf(self, header):
     yaw = np.median(self.buffer)
     q = tfs.quaternion_from_euler(0, 0, yaw - math.pi)
     ts = TransformStamped()
     ts.header = header
     ts.header.frame_id = self.source
     ts.child_frame_id = self.goal
     ts.transform.translation.x = self.position[0]
     ts.transform.translation.y = self.position[1]
     ts.transform.translation.z = 0
     ts.transform.rotation.x = q[0]
     ts.transform.rotation.y = q[1]
     ts.transform.rotation.z = q[2]
     ts.transform.rotation.w = q[3]
     msg = tfMessage()
     msg.transforms.append(ts)
     self.tf_pub.publish(msg)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import header [as 别名]
 def publish_filtered_tf(self, header):
     m = np.median(self.buffer, axis=0)
     if not self.filter_position:
         m[0:3] = self.last_transform[0:3]
     q = quaternion_from_euler(m[3], m[4], m[5])
     ts = TransformStamped()
     ts.header = header
     ts.child_frame_id = self.frame_id + '_filtered'
     ts.transform.translation.x = m[0]
     ts.transform.translation.y = m[1]
     ts.transform.translation.z = m[2]
     ts.transform.rotation.x = q[0]
     ts.transform.rotation.y = q[1]
     ts.transform.rotation.z = q[2]
     ts.transform.rotation.w = q[3]
     msg = tfMessage()
     msg.transforms.append(ts)
     self.tf_pub.publish(msg)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import header [as 别名]
    def callback(self, data):
        rospy.loginfo("Objects %d", data.objects.__len__())

        table_corners = []

        # obtain table_offset and table_pose
        to = rospy.wait_for_message(self.table_topic, MarkerArray);

        # obtain Table corners ...
        rospy.loginfo("Tables hull size %d", to.markers.__len__())
        if not to.markers:
            rospy.loginfo("No tables detected")
            return
        else:
            # NB - D says that ORK has this set to filter based on height.
            # IIRC, it's 0.6-0.8m above whatever frame is set as the floor
            point_array_size = 4      # for the 4 corners of the bounding box
            for i in range (0, point_array_size):
                p = Point()
                p.x = to.markers[0].points[i].x
                p.y = to.markers[0].points[i].y
                p.z = to.markers[0].points[i].z
                table_corners.append(p)
            # this is a table pose at the edge close to the robot, in the center of x axis
            table_pose = PoseStamped()
            table_pose.header = to.markers[0].header
            table_pose.pose = to.markers[0].pose
            
        # Determine table dimensions
        rospy.loginfo('calculating table pose bounding box in frame: %s' % table_pose.header.frame_id)
        min_x = sys.float_info.max
        min_y = sys.float_info.max
        max_x = -sys.float_info.max
        max_y = -sys.float_info.max

        for i in range (table_corners.__len__()):
            if table_corners[i].x > max_x:
                max_x = table_corners[i].x
            if table_corners[i].y > max_y:
                max_y = table_corners[i].y
            if table_corners[i].x < min_x:
                min_x = table_corners[i].x
            if table_corners[i].y < min_y:
                min_y = table_corners[i].y

        table_dim = Point()
        # TODO: if we don't (can't!) compute the height, should we at least give it non-zero depth? 
        # (would also require shifting the observed centroid down by table_dim.z/2)
        table_dim.z = 0.0

        table_dim.x = abs(max_x - min_x)
        table_dim.y = abs(max_y - min_y)
        print "Dimensions: ", table_dim.x, table_dim.y

        # Temporary frame used for transformations
        table_link = 'table_link'

        # centroid of a table in table_link frame
        centroid = PoseStamped()
        centroid.header.frame_id = table_link
        centroid.header.stamp = table_pose.header.stamp
        centroid.pose.position.x = (max_x + min_x)/2.
        centroid.pose.position.y = (max_y + min_y)/2.
        centroid.pose.position.z = 0.0
        centroid.pose.orientation.x = 0.0
        centroid.pose.orientation.y = 0.0
        centroid.pose.orientation.z = 0.0
        centroid.pose.orientation.w = 1.0

        # generate transform from table_pose to our newly-defined table_link
        tt = TransformStamped()
        tt.header = table_pose.header
        tt.child_frame_id = table_link
        tt.transform.translation = table_pose.pose.position
        tt.transform.rotation = table_pose.pose.orientation
        self.tl.setTransform(tt)
        self.tl.waitForTransform(table_link, table_pose.header.frame_id, table_pose.header.stamp, rospy.Duration(3.0))
        if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
            centroid_table_pose = self.tl.transformPose(table_pose.header.frame_id, centroid)
            self.pose_pub.publish(centroid_table_pose)
        else:
            rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
            return

        # transform each object into desired frame; add to list of clusters
        cluster_list = []
        for i in range (data.objects.__len__()):
            rospy.loginfo("Point clouds %s", data.objects[i].point_clouds.__len__())

            pc = PointCloud2()
            pc = data.objects[i].point_clouds[0]
            arr = pointclouds.pointcloud2_to_array(pc, 1)
            arr_xyz = pointclouds.get_xyz_points(arr)

            arr_xyz_trans = []
            for j in range (arr_xyz.__len__()):
                ps = PointStamped();
                ps.header.frame_id = table_link
                ps.header.stamp = table_pose.header.stamp
                ps.point.x = arr_xyz[j][0]
#.........这里部分代码省略.........