Python msg.Pose方法代码示例

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def _retract_loop(self, time=2, hover_distance=None):
        """

        :param time:
        :param hover_distance:
        :return:
        """
        if hover_distance is None:
            hover_distance = self._hover_distance

        # retrieve current pose from endpoint
        current_pose = self.sawyer_robot._limb.endpoint_pose()
        ik_pose = Pose()
        ik_pose.position.x = current_pose['position'].x
        ik_pose.position.y = current_pose['position'].y
        ik_pose.position.z = current_pose['position'].z + hover_distance
        ik_pose.orientation.x = current_pose['orientation'].x
        ik_pose.orientation.y = current_pose['orientation'].y
        ik_pose.orientation.z = current_pose['orientation'].z
        ik_pose.orientation.w = current_pose['orientation'].w
        self.create_linear_motion_task(ik_pose, time=time).result() 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def _publish_grasps(self, grasps):
        """
        Publish grasps as poses, using a PoseArray message
        """

        if self._grasps_pub.get_num_connections() > 0:
            msg = PoseArray()
            msg.header.frame_id = self._robot.get_planning_frame()
            msg.header.stamp = rospy.Time.now()

            for grasp in grasps:
                p = grasp.grasp_pose.pose

                msg.poses.append(Pose(p.position, p.orientation))

            self._grasps_pub.publish(msg) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def on_enter(self, userdata):
        """Create and send action goal"""

        self._arrived = False
        self._failed = False

        # Create and populate action goal
        goal = MoveBaseGoal()

        pt = Point(x = userdata.waypoint.x, y = userdata.waypoint.y)
        qt = transformations.quaternion_from_euler(0, 0, userdata.waypoint.theta)

        goal.target_pose.pose = Pose(position = pt,
                                     orientation = Quaternion(*qt))

        goal.target_pose.header.frame_id = "odom"
        # goal.target_pose.header.stamp.secs = 5.0

        # Send the action goal for execution
        try:
            self._client.send_goal(self._action_topic, goal)
        except Exception as e:
            Logger.logwarn("Unable to send navigation action goal:\n%s" % str(e))
            self._failed = True 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def convert_pose(pose, from_frame, to_frame):
    """
    Convert a pose or transform between frames using tf.
        pose            -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
        from_frame      -> A string that defines the original reference_frame of the robot
        to_frame        -> A string that defines the desired reference_frame of the robot to convert to
    """
    global tfBuffer, listener

    # Create Listener objet to recieve and buffer transformations
    trans = None

    try:
        trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e:
        print(e)
        rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
        return None 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [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) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def align_pose_orientation_to_frame(from_pose, from_reference_frame, to_reference_frame):
    """
    Update the orientation of from_pose so that it matches the orientation of to_reference_frame
    Useful for aligning a desired position pose with a gripper, for example.
        from_pose                   -> A geometry_msgs.msg/Pose to allign
        from_reference_frame        -> A string defining the reference_frame of the pose
        to_reference_frame          -> A string defining the reference_frame to allign to
    """
    # Create transform
    p = gmsg.Pose()
    p.orientation.w = 1.0

    # Convert reference frame orientation from -> to
    pose_orientation = convert_pose(p, to_reference_frame, from_reference_frame)

    # Copy orientation to pose.
    from_pose.orientation = pose_orientation.orientation

    return from_pose 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def set_interaction_frame(self, interaction_frame = default_interaction_frame):
        """
        @param interaction_frame:
          - None:  populate with vector of default values
          - Pose: copy all the elements
        """
        if not isinstance(interaction_frame, Pose):
            rospy.logerr('interaction_frame must be of type geometry_msgs.Pose')
            return

        # check for unit quaternion
        quat_sum_square = (interaction_frame.orientation.w * interaction_frame.orientation.w
                          + interaction_frame.orientation.x * interaction_frame.orientation.x
                          + interaction_frame.orientation.y * interaction_frame.orientation.y
                          + interaction_frame.orientation.z * interaction_frame.orientation.z)
        if quat_sum_square  > 1.0 + 1e-7 or quat_sum_square < 1.0 - 1e-7:
            rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
            return

        self._data.interaction_frame = interaction_frame 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def spawn_model(model_name):
    """ Spawns a model in front of the RGBD camera.

        Args: None

        Returns: None
    """
    initial_pose = Pose()
    initial_pose.position.x = 0
    initial_pose.position.y = 1
    initial_pose.position.z = 1

    # Spawn the new model #
    model_path = rospkg.RosPack().get_path('sensor_stick')+'/models/'
    model_xml = ''

    with open (model_path + model_name + '/model.sdf', 'r') as xml_file:
        model_xml = xml_file.read().replace('\n', '')

    spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
    spawn_model_prox('training_model', model_xml, '', initial_pose, 'world') 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def convert_pose(pose, from_frame, to_frame):
    """
    Convert a pose or transform between frames using tf.
        pose            -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
        from_frame      -> A string that defines the original reference_frame of the robot
        to_frame        -> A string that defines the desired reference_frame of the robot to convert to
    """
    global tfBuffer, listener

    if tfBuffer is None or listener is None:
        _init_tf()

    try:
        trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e:
        print(e)
        rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
        return None 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [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
    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) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def __init__(self):

        rospy.init_node("mav_control_node")
        rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback)
        rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback)

        self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1)
        self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1)
        self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1)

        # mode 0 = STABILIZE
        # mode 4 = GUIDED
        # mode 9 = LAND
        self.mode_service = rospy.ServiceProxy('/mavros/set_mode', SetMode)
        self.arm_service = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
        self.takeoff_service = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)

        self.rc = RCIn()
        self.pose = Pose()
        self.timestamp = rospy.Time() 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def centroid_callback(self, data):
        self.goal_poses = []

        for blob in data.blobs:
            centroid = (blob.centroid.x, blob.centroid.y)
            
            if blob.axis is None or self.camera_model is None:
                return
            axis = numpy.concatenate((numpy.array(unmap(blob.axis.points[0])),\
                                      numpy.array(unmap(blob.axis.points[1]))))

            pos = self.solve_goal_pose(centroid)

            #Calculate desired orientation
            theta = self.calculate_angle(axis)
            quat = tf.transformations.quaternion_from_euler(-pi, 0, theta)

            self.goal_poses.append( Pose(position=Point(*pos), orientation=Quaternion(*quat)))
        self.done = True 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def __init__(self, image_path=None):
        height = int(rospy.get_param("~grid_height", 800))
        width = int(rospy.get_param("~grid_width", 800))
        resolution = rospy.get_param("~grid_resolution", .25)
        ogrid_topic = rospy.get_param("/lqrrt_node/ogrid_topic", "/ogrid")

        self.grid_drawer = DrawGrid(height, width, image_path)
        self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1)

        m = MapMetaData()
        m.resolution = resolution
        m.width = width
        m.height = height
        pos = np.array([-width * resolution / 2, -height * resolution / 2, 0])
        quat = np.array([0, 0, 0, 1])
        m.origin = Pose()
        m.origin.position.x, m.origin.position.y = pos[:2]
        self.map_meta_data = m

        rospy.Timer(rospy.Duration(1), self.pub_grid) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def from_ros_pose_msg(pose_msg,
                          from_frame='unassigned',
                          to_frame='world'):
        """Creates a RigidTransform from a ROS pose msg. 
        
        Parameters
        ----------
        pose_msg : :obj:`geometry_msgs.msg.Pose`
            ROS pose message
        """
        quaternion = np.array([pose_msg.orientation.w,
                               pose_msg.orientation.x,
                               pose_msg.orientation.y,
                               pose_msg.orientation.z])
        position = np.array([pose_msg.position.x,
                             pose_msg.position.y,
                             pose_msg.position.z])
        pose = RigidTransform(rotation=quaternion,
                              translation=position,
                              from_frame=from_frame,
                              to_frame=to_frame)
        return pose 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Pose [as 别名]
def list_to_pose(pose_list):
    pose_msg = Pose()
    if len(pose_list) == 7:
        pose_msg.position.x = pose_list[0]
        pose_msg.position.y = pose_list[1]
        pose_msg.position.z = pose_list[2]
        pose_msg.orientation.x = pose_list[3]
        pose_msg.orientation.y = pose_list[4]
        pose_msg.orientation.z = pose_list[5]
        pose_msg.orientation.w = pose_list[6]
    elif len(pose_list) == 6:
        pose_msg.position.x = pose_list[0]
        pose_msg.position.y = pose_list[1]
        pose_msg.position.z = pose_list[2]
        q = tf.transformations.quaternion_from_euler(
            pose_list[3], pose_list[4], pose_list[5]
        )
        pose_msg.orientation.x = q[0]
        pose_msg.orientation.y = q[1]
        pose_msg.orientation.z = q[2]
        pose_msg.orientation.w = q[3]
    return pose_msg