Python msg.Vector3方法代码示例

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [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 Vector3 [as 别名]
def carla_location_to_ros_vector3(carla_location):
    """
    Convert a carla location to a ROS vector3

    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 vector3
    :rtype: geometry_msgs.msg.Vector3
    """
    ros_translation = Vector3()
    ros_translation.x = carla_location.x
    ros_translation.y = -carla_location.y
    ros_translation.z = carla_location.z

    return ros_translation 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def carla_vector_to_ros_vector_rotated(carla_vector, carla_rotation):
    """
    Rotate carla vector, return it as ros vector

    :param carla_vector: the carla vector
    :type carla_vector: carla.Vector3D
    :param carla_rotation: the carla rotation
    :type carla_rotation: carla.Rotation
    :return: rotated ros vector
    :rtype: Vector3
    """
    rotation_matrix = carla_rotation_to_numpy_rotation_matrix(carla_rotation)
    tmp_array = rotation_matrix.dot(numpy.array([carla_vector.x, carla_vector.y, carla_vector.z]))
    ros_vector = Vector3()
    ros_vector.x = tmp_array[0]
    ros_vector.y = -tmp_array[1]
    ros_vector.z = tmp_array[2]
    return ros_vector 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def test_vehicle_info(self):
        """
        Tests vehicle_info
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/vehicle_info", CarlaEgoVehicleInfo, timeout=TIMEOUT)
        self.assertNotEqual(msg.id, 0)
        self.assertEqual(msg.type, "vehicle.tesla.model3")
        self.assertEqual(msg.rolename, "ego_vehicle")
        self.assertEqual(len(msg.wheels), 4)
        self.assertNotEqual(msg.max_rpm, 0.0)
        self.assertNotEqual(msg.moi, 0.0)
        self.assertNotEqual(msg.damping_rate_full_throttle, 0.0)
        self.assertNotEqual(msg.damping_rate_zero_throttle_clutch_engaged, 0.0)
        self.assertNotEqual(
            msg.damping_rate_zero_throttle_clutch_disengaged, 0.0)
        self.assertTrue(msg.use_gear_autobox)
        self.assertNotEqual(msg.gear_switch_time, 0.0)
        self.assertNotEqual(msg.mass, 0.0)
        self.assertNotEqual(msg.clutch_strength, 0.0)
        self.assertNotEqual(msg.drag_coefficient, 0.0)
        self.assertNotEqual(msg.center_of_mass, Vector3()) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def numpy_to_transform(arr):
	from tf import transformations

	shape, rest = arr.shape[:-2], arr.shape[-2:]
	assert rest == (4,4)

	if len(shape) == 0:
		trans = transformations.translation_from_matrix(arr)
		quat = transformations.quaternion_from_matrix(arr)

		return Transform(
			translation=Vector3(*trans),
			rotation=Quaternion(*quat)
		)
	else:
		res = np.empty(shape, dtype=np.object_)
		for idx in np.ndindex(shape):
			res[idx] = Transform(
				translation=Vector3(*transformations.translation_from_matrix(arr[idx])),
				rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
			) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def numpy_to_pose(arr):
	from tf import transformations

	shape, rest = arr.shape[:-2], arr.shape[-2:]
	assert rest == (4,4)

	if len(shape) == 0:
		trans = transformations.translation_from_matrix(arr)
		quat = transformations.quaternion_from_matrix(arr)

		return Pose(
			position=Vector3(*trans),
			orientation=Quaternion(*quat)
		)
	else:
		res = np.empty(shape, dtype=np.object_)
		for idx in np.ndindex(shape):
			res[idx] = Pose(
				position=Vector3(*transformations.translation_from_matrix(arr[idx])),
				orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
			) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def test_transform(self):
        t = Transform(
            translation=Vector3(1, 2, 3),
            rotation=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(Transform, t_mat)

        np.testing.assert_allclose(msg.translation.x, t.translation.x)
        np.testing.assert_allclose(msg.translation.y, t.translation.y)
        np.testing.assert_allclose(msg.translation.z, t.translation.z)
        np.testing.assert_allclose(msg.rotation.x, t.rotation.x)
        np.testing.assert_allclose(msg.rotation.y, t.rotation.y)
        np.testing.assert_allclose(msg.rotation.z, t.rotation.z)
        np.testing.assert_allclose(msg.rotation.w, t.rotation.w) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def setpoint_position(self, position, yaw):
        self.setpoint_raw.type_mask = self.type_mask_Fly
        self.setpoint_raw.velocity = Vector3()
        self.setpoint_raw.position = position
        self.setpoint_raw.yaw = yaw+yaw_bug

        self.setpoint_local.pose.position = position
        q = quaternion_from_euler(0,0, yaw+self.yaw_bug,axes="sxyz")
        self.setpoint_local.pose.orientation.x = q[0]
        self.setpoint_local.pose.orientation.y = q[1]
        self.setpoint_local.pose.orientation.z = q[2]
        self.setpoint_local.pose.orientation.w = q[3] 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def setpoint_takeoff_here_position(self, altitude):
        self.setpoint_raw.type_mask = self.type_mask_Fly
        self.setpoint_raw.velocity = Vector3()
        self.setpoint_raw.position = self.local_position
        self.setpoint_raw.position.z = altitude
        self.setpoint_raw.yaw = self.local_yaw

        self.setpoint_local.pose.position = self.local_position
        self.setpoint_local.pose.position.z = altitude
        self.setpoint_local.pose.orientation = self.quaternion 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def setpoint_land_here_position(self):
        self.setpoint_raw.type_mask = self.type_mask_Fly
        self.setpoint_raw.velocity = Vector3()
        self.setpoint_raw.position = self.local_position
        self.setpoint_raw.position.z = 0.3
        self.setpoint_raw.yaw = self.local_yaw

        self.setpoint_local.pose.position = self.local_position
        self.setpoint_local.pose.orientation = self.quaternion 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def setpoint_takeoff(self):
        self.setpoint_raw.type_mask = self.type_mask_Takeoff
        self.setpoint_raw.velocity = Vector3(0.0,self.takeoff_speed,0.0)

        self.setpoint_local.pose.position = self.local_position
        self.setpoint_local.pose.position.z = self.takeoff_altitude+0.5
        self.setpoint_local.pose.orientation = self.quaternion 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def setpoint_land(self):
        self.setpoint_raw.type_mask = self.type_mask_Land
        self.setpoint_raw.velocity = Vector3(0.0,self.landing_speed,0.0)

        self.setpoint_local.pose.position = self.local_position
        self.setpoint_local.pose.position.z = self.landing_altitude
        self.setpoint_local.pose.orientation = self.quaternion 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def set_velocity(x, phi, turtle_name, logger):
    position_vector = Vector3(x, 0, 0)
    rotation_vector = Vector3(0, 0, phi)
    twist_msg = Twist(position_vector, rotation_vector)
    try:
        logger.info("move_to_position: publish twist to turtle".format(turtle_name))
        turtle_vel_publisher = rospy.Publisher("/" + turtle_name + "/cmd_vel", Twist, queue_size=10, latch=True)
        turtle_vel_publisher.publish(twist_msg)
        rate = rospy.Rate(10)
        rate.sleep()
    except rospy.ROSInterruptException as e:
        logger.error("Failed to send a velocity command to turtle {}: {}".format(turtle_name, str(e))) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def execute(self, inputs, outputs, gvm):
    turtle_name = inputs["turtle_name"]
    x = inputs["x_vel"]
    phi = inputs["phi_vel"]
    rate = rospy.Rate(10)
    position_vector = Vector3(x, 0, 0)
    rotation_vector = Vector3(0, 0, phi)
    twist_msg = Twist(position_vector, rotation_vector)
    self.logger.info("moving turtle {} {}".format(str(x), str(phi)))
    self.logger.info("publish twist to turtle {}".format(turtle_name))
    turtle_vel_publisher = rospy.Publisher("/" + turtle_name + "/cmd_vel", Twist, queue_size=10, latch=True)
    turtle_vel_publisher.publish(twist_msg)
    rate.sleep()

    return 0 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import Vector3 [as 别名]
def pubPosCmd(self):
        """
        Publish the position command for the robot
        x, y, yaw are in the global frame
        """
        assert self.enabled_pos_controller and self.enabled_velocity_controller, \
            "pos_controller and velocity_controller should be both enabled to execute positional command"
        msg = Vector3(
            self.robot_pos_cmd[0], self.robot_pos_cmd[1], self.robot_yaw_cmd)
        self.pos_cmd_pub.publish(msg)
        self.move_finished = False
        print("move to x: {:.4f} y:{:.4f} yaw: {:.4f}".format(
            msg.x, msg.y, msg.z))