Python msg.Point方法代碼示例

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def navigation():
    pub = rospy.Publisher('usv_position_setpoint', Odometry, queue_size=10) # only create a rostopic, navigation system TODO
    rospy.init_node('navigation_publisher')
    rate = rospy.Rate(60) # 10h

    x = -20.0
    y = -20.0

    msg = Odometry()
   # msg.header = Header()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = "navigation"
    msg.pose.pose.position = Point(x, y, 0.)
 
    while not rospy.is_shutdown():
            pub.publish(msg)
            rate.sleep() 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [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 Point [as 別名]
def setpoint_init(self):
        # type_mask
        # 2552 : XYZ & yaw - POSITION
        # 7104 : XYZ, yaw, vXYZ, TAKE_OFF_SETPOINT
        # 3064 : 0000 1001 1111 1000
        self.setpoint_raw = PositionTarget()
        self.setpoint_raw.coordinate_frame = self.setpoint_raw.FRAME_LOCAL_NED
        self.setpoint_raw.type_mask = self.type_mask_Fly
        self.setpoint_raw.position = Point()
        self.setpoint_raw.yaw = 0.0
        self.setpoint_raw.velocity = Vector3()
        self.setpoint_raw.acceleration_or_force = Vector3()
        self.setpoint_raw.yaw_rate = 0.0
        self.setpoint_local = PoseStamped()
        self.setpoint_local.pose.orientation.w = 1
        self.laser_position_count = 0 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [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 Point [as 別名]
def carla_location_to_ros_point(carla_location):
    """
    Convert a carla location to a ROS point

    Considers the conversion from left-handed system (unreal) to right-handed
    system (ROS)

    :param carla_location: the carla location
    :type carla_location: carla.Location
    :return: a ROS point
    :rtype: geometry_msgs.msg.Point
    """
    ros_point = Point()
    ros_point.x = carla_location.x
    ros_point.y = -carla_location.y
    ros_point.z = carla_location.z

    return ros_point 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def GetCurrentRobotPose(self,frame="map"):
        self.tfl.waitForTransform(frame, "base_link", rospy.Time(), rospy.Duration(1.0))
        (trans,rot) = self.tfl.lookupTransform(frame, "base_link", rospy.Time(0))
        
        """
        Remove all the rotation components except yaw
        """
        euler = tf.transformations.euler_from_quaternion(rot)
        rot = tf.transformations.quaternion_from_euler(0,0,euler[2])    
    
        current_pose = PoseWithCovarianceStamped()
        current_pose.header.stamp = rospy.get_rostime()
        current_pose.header.frame_id = frame
        current_pose.pose.pose = Pose(Point(trans[0], trans[1], 0.0), Quaternion(rot[0],rot[1],rot[2],rot[3]))
        
        return current_pose 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def test_pose(self):
        t = Pose(
            position=Point(1.0, 2.0, 3.0),
            orientation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0))
        )

        t_mat = ros_numpy.numpify(t)

        np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0])

        msg = ros_numpy.msgify(Pose, t_mat)

        np.testing.assert_allclose(msg.position.x, t.position.x)
        np.testing.assert_allclose(msg.position.y, t.position.y)
        np.testing.assert_allclose(msg.position.z, t.position.z)
        np.testing.assert_allclose(msg.orientation.x, t.orientation.x)
        np.testing.assert_allclose(msg.orientation.y, t.orientation.y)
        np.testing.assert_allclose(msg.orientation.z, t.orientation.z)
        np.testing.assert_allclose(msg.orientation.w, t.orientation.w) 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def goal_pose(pose):
    goal_pose = Odometry()
    goal_pose.header.stamp = rospy.Time.now()
    goal_pose.header.frame_id = 'world'
    goal_pose.pose.pose.position = Point(pose[0][0]+x_offset, pose[0][1]+y_offset, 0.)
    return goal_pose 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def angleToPoint(lat, lon):
    tmp = Point()
    tmp.x = lat
    tmp.y = lon

    return tmp 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def homotransform_to_pose_msg(homotransform):
    trans = tf.transformations.translation_from_matrix(homotransform)
    quat = tf.transformations.quaternion_from_matrix(homotransform)
    return Pose(
        position=Point(x=trans[0], y=trans[1], z=trans[2]),
        orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def update_table1_collision(self):
        table1pose = geometry_msgs.msg.PoseStamped()
        table1pose.pose = Pose(position=Point(x=0.75, y=0.0, z=self.table1_z))
        table1pose.pose.orientation.w = 1.0
        table1pose.header.stamp = rospy.Time.now()
        table1pose.header.frame_id = "world"
        self.scene.add_box("table1", table1pose, size=self.tableshape)

        self.update_table_edges_collision(table1pose, 0.15,"table1",self.table1_z) 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def update_table2_collision(self):
        table2pose = geometry_msgs.msg.PoseStamped()
        table2pose.pose = Pose(position=Point(x=0.0, y=1.0, z=self.table2_z))
        table2pose.pose.orientation.w = 1.0
        table2pose.header.stamp = rospy.Time.now()
        table2pose.header.frame_id = "world"
        edgeheight = 0.15

        self.scene.add_box("table2", table2pose, size=self.tableshape)

        self.update_table_edges_collision(table2pose, edgeheight,"table2",self.table2_z)

        splits = 3
        for i in xrange(1, splits):
            edgeyi = geometry_msgs.msg.PoseStamped()
            edgeyi.pose = Pose(
                position=Point(x=i * self.tableshape[0] / float(splits) - self.tableshape[0] * 0.5, y=1.0,
                               z=self.table2_z + edgeheight * 0.5))
            edgeyi.pose.orientation.w = 1.0
            edgeyi.header.stamp = rospy.Time.now()
            edgeyi.header.frame_id = "world"

            newshape = (
            self.tableshape[0] * 0.01, self.tableshape[1], self.tableshape[2] + edgeheight * 0.5)
            self.scene.add_box("table2_edgey_" + str(i), edgeyi, size=newshape)


        splits = 2
        for i in xrange(1, splits):
            edgexi = geometry_msgs.msg.PoseStamped()
            edgexi.pose = Pose(
                position=Point(x= 0.0, y=1.0 - self.tableshape[1] * 0.5 + i * self.tableshape[1] / float(splits), z=self.table2_z + edgeheight * 0.5))
            edgexi.pose.orientation.w = 1.0
            edgexi.header.stamp = rospy.Time.now()
            edgexi.header.frame_id = "world"

            newshape = (
            self.tableshape[0] , self.tableshape[1] * 0.01, self.tableshape[2] + edgeheight * 0.5)
            self.scene.add_box("table2_edgex_" + str(i), edgexi, size=newshape) 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def update_ceiling_obstacle(self):
        ceilshape = (2.0, 2.0, 0.02)

        ceil1pose = geometry_msgs.msg.PoseStamped()
        ceil1pose.pose = Pose(position=Point(x=0.0, y=0.0, z=self.ceilheight))
        ceil1pose.pose.orientation.w = 1.0
        ceil1pose.header.stamp = rospy.Time.now()
        ceil1pose.header.frame_id = self.robot.get_planning_frame()
        self.scene.add_box("ceil1", ceil1pose, size=ceilshape) 

# 需要導入模塊: from geometry_msgs import msg [as 別名]
# 或者: from geometry_msgs.msg import Point [as 別名]
def get_position(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return Point(*trans)