Python geometry_msgs.msg.Point() Examples

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 

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() 

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 

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) 

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 

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 

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 

def setPose(self, x, y, z, phi, theta, psi):
    orient = \
      Quaternion(*tf.transformations.quaternion_from_euler(phi, theta, psi))
    pose = Pose(Point(x, y, z), orient)
    self.left_arm.set_pose_target(pose)
    self.left_arm.go(True) 

def is_left_cones(trans, cones):
    A = Point(cones[0][0], cones[0][1])
    B = Point(cones[-1][0], cones[-1][1])
    return is_left(trans, A, B) 

def get_track_init_pos(self):
        self.pose_init = Point(self.track_details['starting_pose_front_wing'])
        yaw = self.track_details['starting_pose_front_wing'][2]
        qt = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
        return self.pose_init, Quaternion(*qt) 

def changeSpawned(self, config=None) :
		if config is None :
			config = self.currentConfiguration

		for elem in config.shape_list :
			modelstate = ModelState()

			elpose = elem.shape_pose.tolist()[0]
			
			pose = Point()
			quat = Quaternion()

			pose.x = elpose[0]
			pose.y = elpose[1]
			pose.z = elpose[2]
			modelstate.pose.position = pose

			quaternion = tf.transformations.quaternion_from_euler(
				elpose[3],
				elpose[4],
				elpose[5]
				)
			modelstate.pose.orientation.x = quaternion[0]
			modelstate.pose.orientation.y = quaternion[1]
			modelstate.pose.orientation.z = quaternion[2]
			modelstate.pose.orientation.w = quaternion[3]

			elname = elem.getName()
			modelstate.model_name = elname


			self.pub_modelstate.publish(modelstate)

			#rospy.loginfo('CONFIGURATION SPAWNED : making changes... :\n {}'.format(modelstate) )
			#time.sleep(0.1) 

def __init__(self, grid_map):
        self.map = grid_map
        self.width = grid_map.info.width
        self.height = grid_map.info.height
        self.resolution = grid_map.info.resolution

        self.origin = Point()
        self.origin.x = grid_map.info.origin.position.x
        self.origin.y = grid_map.info.origin.position.y 

def _get_current_pose(self):

        """
        Gets the current pose of the base frame in the global frame
        """
        current_pose = None
        listener = tf.TransformListener()
        rospy.sleep(1.0)
        try:
            listener.waitForTransform(self.global_frame, self.base_frame, rospy.Time(), rospy.Duration(1.0))
        except:
            pass
        try:
            (trans,rot) = listener.lookupTransform(self.global_frame, self.base_frame, rospy.Time(0))

            pose_parts = [0.0] * 7
            pose_parts[0]  = trans[0]
            pose_parts[1]  = trans[1]
            pose_parts[2]  = 0.0
            euler = tf.transformations.euler_from_quaternion(rot)
            rot = tf.transformations.quaternion_from_euler(0,0,euler[2])
            pose_parts[3] = rot[0]
            pose_parts[4] = rot[1]
            pose_parts[5] = rot[2]
            pose_parts[6] = rot[3]

            current_pose = PoseWithCovarianceStamped()
            current_pose.header.stamp = rospy.get_rostime()
            current_pose.header.frame_id = self.global_frame
            current_pose.pose.pose = Pose(Point(pose_parts[0], pose_parts[1], pose_parts[2]), Quaternion(pose_parts[3],pose_parts[4],pose_parts[5],pose_parts[6]))
        except:
            rospy.loginfo("Could not get transform from %(1)s->%(2)s"%{"1":self.global_frame,"2":self.base_frame})

        return current_pose 

def run_step(self, current_pose, waypoint, dt):
        """
        Estimate the steering angle of the vehicle based on the PID equations

        :param waypoint: target waypoint
        :param current_pose: current pose of the vehicle
        :return: steering control in the range [-1, 1]
        """
        v_begin = current_pose.position
        quaternion = (
            current_pose.orientation.x,
            current_pose.orientation.y,
            current_pose.orientation.z,
            current_pose.orientation.w
        )
        _, _, yaw = euler_from_quaternion(quaternion)
        v_end = Point()
        v_end.x = v_begin.x + math.cos(yaw)
        v_end.y = v_begin.y + math.sin(yaw)

        v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0])
        w_vec = np.array([waypoint.position.x -
                          v_begin.x, waypoint.position.y -
                          v_begin.y, 0.0])
        _dot = math.acos(np.clip(np.dot(w_vec, v_vec) /
                                 (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0))

        _cross = np.cross(v_vec, w_vec)
        if _cross[2] < 0:
            _dot *= -1.0

        self._e_buffer.append(_dot)
        if len(self._e_buffer) >= 2:
            _de = (self._e_buffer[-1] - self._e_buffer[-2]) / dt
            _ie = sum(self._e_buffer) * dt
        else:
            _de = 0.0
            _ie = 0.0

        return np.clip((self._K_P * _dot) + (self._K_D * _de /
                                             dt) + (self._K_I * _ie * dt), -1.0, 1.0) 

def is_left(trans, A, B):
    return ccw(A, B, Point(trans[0], trans[1])) 

def _make_mesh(self, c, scale = (1, 1, 1)):
    '''
    This was taken from moveit commander and slightly modified.
    '''
    filename = c.geometry.filename
    if pyassimp is False:
        raise RuntimeError("Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt")
    scene = pyassimp.load(filename)
    if not scene.meshes or len(scene.meshes) == 0:
        raise MoveItCommanderException("There are no meshes in the file")
    if len(scene.meshes[0].faces) == 0:
        raise MoveItCommanderException("There are no faces in the mesh")
    
    mesh = Mesh()
    first_face = scene.meshes[0].faces[0]
    if hasattr(first_face, '__len__'):
        for face in scene.meshes[0].faces:
            if len(face) == 3:
                triangle = MeshTriangle()
                triangle.vertex_indices = [face[0], face[1], face[2]]
                mesh.triangles.append(triangle)
    elif hasattr(first_face, 'indices'):
        for face in scene.meshes[0].faces:
            if len(face.indices) == 3:
                triangle = MeshTriangle()
                triangle.vertex_indices = [face.indices[0],
                                           face.indices[1],
                                           face.indices[2]]
                mesh.triangles.append(triangle)
    else:
        raise RuntimeError("Unable to build triangles from mesh due to mesh object structure")
    for vertex in scene.meshes[0].vertices:
        point = Point()
        point.x = vertex[0]*scale[0]
        point.y = vertex[1]*scale[1]
        point.z = vertex[2]*scale[2]
        mesh.vertices.append(point)
    pyassimp.release(scene)
    return mesh 

def numpy_to_point(arr):
	if arr.shape[-1] == 4:
		arr = arr[...,:-1] / arr[...,-1]

	if len(arr.shape) == 1:
		return Point(*arr)
	else:
		return np.apply_along_axis(lambda v: Point(*v), axis=-1, arr=arr) 

def setPose(self, arm, x, y, z, phi, theta, psi):
    if arm != 'left' and arm != 'right':
      raise ValueError("unknown arm: '%s'" % arm)
    orient = \
      Quaternion(*tf.transformations.quaternion_from_euler(phi, theta, psi)) # <1>
    pose = Pose(Point(x, y, z), orient)
    self.group[arm].set_pose_target(pose)
    self.group[arm].go(True) 

def getPoseFromMatrix(matrix):
    trans, quat = getTfFromMatrix(numpy.linalg.inv(matrix))
    return Pose(position=Point(*trans), orientation=Quaternion(*quat)) 

def centroid_callback(self, data):
        if self.pc is None:
            return
        self.goal_poses = []
        
        for blob in data.blobs:
            centroid = (blob.centroid.x, blob.centroid.y)
            axis = numpy.concatenate((numpy.array(unmap(blob.axis.points[0])),\
                                      numpy.array(unmap(blob.axis.points[1]))))
            pos = self.solve_goal_point(centroid)

            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))) 

def setUp(self):
        self.trajectory = LinearTrajectory(1, 0, 2, 0)
        self.expected_position = Point() 

def setUp(self):
        self.trajectory = LissajousTrajectory(1, 1, 3, 2, 4)
        self.expected_position = Point() 

def setUp(self):
        self.trajectory = SquaredTrajectory(3, 4)
        self.expected_position = Point() 

def setUp(self):
        self.trajectory = EpitrochoidTrajectory(5, 1, 3, 4, 1.0 / 3)
        self.expected_position = Point() 

def test_point(self):
        p = Point(1, 2, 3)

        p_arr = ros_numpy.numpify(p)
        np.testing.assert_array_equal(p_arr, [1, 2, 3])

        p_arrh = ros_numpy.numpify(p, hom=True)
        np.testing.assert_array_equal(p_arrh, [1, 2, 3, 1])

        self.assertEqual(p, ros_numpy.msgify(Point, p_arr))
        self.assertEqual(p, ros_numpy.msgify(Point, p_arrh))
        self.assertEqual(p, ros_numpy.msgify(Point, p_arrh * 2)) 

def setUp(self):
        self.trajectory = LemniscateTrajectory(5, 4)
        self.expected_position = Point() 

def run(self, UAV):
        self.rate.sleep()
        UAV.home = Point(UAV.local_position.x, UAV.local_position.y, UAV.local_position.z)
        UAV.setpoint_raw.position = Point(UAV.local_position.x, UAV.local_position.y, UAV.local_position.z)
        UAV.setpoint_local.pose.position = Point(UAV.local_position.x, UAV.local_position.y, UAV.local_position.z)
        return self.isDone(UAV) 

def local_position_callback(self, local):
        self.quaternion = local.pose.orientation
        self.local_position = Point(local.pose.position.x, local.pose.position.y,local.pose.position.z)
        q = (local.pose.orientation.x, local.pose.orientation.y, local.pose.orientation.z, local.pose.orientation.w)
        _, _, yaw = euler_from_quaternion(q, axes="sxyz")
        self.local_yaw = yaw 

def init():
    global local_pose, yaw
    local_pose = Point()
    yaw = 0
    pass 

def matrix_to_pose(matrix):
    """4x4 matrix to geometry_msgs.msg.Pose"""
    t, q = matrix_to_tf(matrix)
    return Pose(Point(*t), Quaternion(*q))