Python geometry_msgs.msg.Vector3() Examples

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

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) 

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

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

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 

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 

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 

def replicateTransformOverTime(self, transf, orig_frame, dest_frame, frequency, start_time=None, end_time=None):
        """
        Adds a new transform to the graph with the specified value

        This can be useful to add calibration a-posteriori.

        :param transf: value of the transform
        :param orig_frame: the source tf frame of the transform of interest
        :param dest_frame: the target tf frame of the transform of interest
        :param frequency: frequency at which the transform should be published
        :param start_time: the time the transform should be published from
        :param end_time: the time the transform should be published until
        :return:
        """
        if start_time is None:
            start_time = self.getStartTime()
        if end_time is None:
            end_time = self.getEndTime()
        transl, quat = transf
        time_delta = rospy.Duration(1 / frequency)

        t_msg = TransformStamped(header=Header(frame_id=orig_frame),
                                 child_frame_id=dest_frame,
                                 transform=Transform(translation=Vector3(*transl), rotation=Quaternion(*quat)))

        def createMsg(time_nsec):
            time = rospy.Time(time_nsec / 1000000000)
            t_msg2 = copy.deepcopy(t_msg)
            t_msg2.header.stamp = time
            return t_msg2

        new_msgs = [createMsg(t) for t in range(start_time.to_nsec(), end_time.to_nsec(), time_delta.to_nsec())]
        self.tf_messages += new_msgs
        self.tf_messages.sort(key=lambda tfm: tfm.header.stamp.to_nsec())
        self.tf_times = np.array(list((tfm.header.stamp.to_nsec() for tfm in self.tf_messages)))
        self.all_transform_tuples.add((orig_frame, dest_frame)) 

def test_marker(self):
        """
        Tests marker
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message("/carla/marker", Marker, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "ego_vehicle")
        self.assertNotEqual(msg.id, 0)
        self.assertEqual(msg.type, 1)
        self.assertNotEqual(msg.pose, Pose())
        self.assertNotEqual(msg.scale, Vector3())
        self.assertEqual(msg.color.r, 0.0)
        self.assertEqual(msg.color.g, 255.0)
        self.assertEqual(msg.color.b, 0.0) 

def run(self):
        """

        Control loop

        :return:
        """
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            if self._waypoints:
                control = CarlaWalkerControl()
                direction = Vector3()
                direction.x = self._waypoints[0].position.x - self._current_pose.position.x
                direction.y = self._waypoints[0].position.y - self._current_pose.position.y
                direction_norm = math.sqrt(direction.x**2 + direction.y**2)
                if direction_norm > CarlaWalkerAgent.MIN_DISTANCE:
                    control.speed = self._target_speed
                    control.direction.x = direction.x / direction_norm
                    control.direction.y = direction.y / direction_norm
                else:
                    self._waypoints = self._waypoints[1:]
                    if self._waypoints:
                        rospy.loginfo("next waypoint: {} {}".format(self._waypoints[0].position.x, self._waypoints[0].position.y))
                    else:
                        rospy.loginfo("Route finished.")
                self.control_publisher.publish(control)
            try:
                r.sleep()
            except rospy.ROSInterruptException:
                pass 

def resetOdom(self, x, y, yaw):
        """
        The odometry of robot will be reset to x, y, yaw (in global frame)
        """
        msg = Vector3()
        msg.x = x
        msg.y = y
        msg.z = yaw
        self.reset_odom_pub.publish(msg) 

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

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 

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

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 

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 

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 

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] 

def numpy_to_vector3(arr):
	if arr.shape[-1] == 4:
		assert np.all(arr[...,-1] == 0)
		arr = arr[...,:-1]

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

def test_vector3(self):
        v = Vector3(1, 2, 3)

        v_arr = ros_numpy.numpify(v)
        np.testing.assert_array_equal(v_arr, [1, 2, 3])

        v_arrh = ros_numpy.numpify(v, hom=True)
        np.testing.assert_array_equal(v_arrh, [1, 2, 3, 0])

        self.assertEqual(v, ros_numpy.msgify(Vector3, v_arr))
        self.assertEqual(v, ros_numpy.msgify(Vector3, v_arrh))

        with self.assertRaises(AssertionError):
            ros_numpy.msgify(Vector3, np.array([0, 0, 0, 1])) 

def run(self):
        while self.is_looping():
            navigation_tf_msg = TFMessage()
            try:
                motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True))
                localization = self.navigation.getRobotPositionInMap()
                exploration_path = self.navigation.getExplorationPath()
            except Exception as e:
                navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D()))
                self.publishers["map_tf"].publish(navigation_tf_msg)
                self.rate.sleep()
                continue
            if len(localization) > 0 and len(localization[0]) == 3:
                navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2])
                navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot)
                navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion))
            self.publishers["map_tf"].publish(navigation_tf_msg)
            if len(localization) > 0:
                if self.publishers["uncertainty"].get_num_connections() > 0:
                    uncertainty = Marker()
                    uncertainty.header.frame_id = "/base_footprint"
                    uncertainty.header.stamp = rospy.Time.now()
                    uncertainty.type = Marker.CYLINDER
                    uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2)
                    uncertainty.pose.position = Point(0, 0, 0)
                    uncertainty.pose.orientation = self.get_ros_quaternion(
                        almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1))
                    uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5)
                    self.publishers["uncertainty"].publish(uncertainty)
            if self.publishers["map"].get_num_connections() > 0:
                aggregated_map = None
                try:
                    aggregated_map = self.navigation.getMetricalMap()
                except Exception as e:
                    print("error " + str(e))
                if aggregated_map is not None:
                    map_marker = OccupancyGrid()
                    map_marker.header.stamp = rospy.Time.now()
                    map_marker.header.frame_id = "/map"
                    map_marker.info.resolution = aggregated_map[0]
                    map_marker.info.width = aggregated_map[1]
                    map_marker.info.height = aggregated_map[2]
                    map_marker.info.origin.orientation = self.get_ros_quaternion(
                        almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1))
                    map_marker.info.origin.position.x = aggregated_map[3][0]
                    map_marker.info.origin.position.y = aggregated_map[3][1]
                    map_marker.data = aggregated_map[4]
                    self.publishers["map"].publish(map_marker)
            if self.publishers["exploration_path"].get_num_connections() > 0:
                path = Path()
                path.header.stamp = rospy.Time.now()
                path.header.frame_id = "/map"
                for node in exploration_path:
                    current_node = PoseStamped()
                    current_node.pose.position.x = node[0]
                    current_node.pose.position.y = node[1]
                    path.poses.append(current_node)
                self.publishers["exploration_path"].publish(path)
            self.rate.sleep() 

def __init__(self, init_x, init_y, init_yaw):
        """
        Class for controlling omnirobot based on topic mechanism of ros
        """

        super(OmniRobot, self).__init__()

        # Initialize the direction
        self.init_pos = [init_x, init_y]
        self.init_yaw = init_yaw

        # OmniRobot's real position on the grid
        self.robot_pos = [0, 0]
        self.robot_yaw = 0  # in rad

        # OmniRobot's position command on the grid
        self.robot_pos_cmd = self.init_pos[:]
        self.robot_yaw_cmd = self.init_yaw

        # Target's real position on the grid
        self.target_pos = [0, 0]
        self.target_yaw = 0

        # status of moving
        self.move_finished = False
        self.target_pos_changed = False

        # Distance for each step
        self.step_distance = STEP_DISTANCE

        self.visual_robot_sub = rospy.Subscriber(
            "/visual_robot_pose", PoseStamped, self.visualRobotCallback, queue_size=10)
        self.visual_target_sub = rospy.Subscriber(
            "/visual_target_pose", PoseStamped, self.visualTargetCallback, queue_size=10)

        self.pos_cmd_pub = rospy.Publisher(
            "/position_commands", Vector3, queue_size=10)
        self.move_finished_sub = rospy.Subscriber(
            "/finished", Bool, self.moveFinishedCallback, queue_size=10)
        self.stop_signal_pub = rospy.Publisher("/stop", Bool, queue_size=10)
        self.reset_odom_pub = rospy.Publisher(
            "/reset_odom", Vector3, queue_size=10)
        self.reset_signal_pub = rospy.Publisher("/reset", Bool, queue_size=10)
        self.switch_velocity_controller_pub = rospy.Publisher("/switch/velocity_controller", Bool, queue_size=10)
        self.switch_pos_controller_pub = rospy.Publisher("/switch/pos_controller", Bool, queue_size=10)

        self.wheel_speeds_command_pub = rospy.Publisher("/wheel_speeds_commands", WheelSpeeds, queue_size=10)
        self.velocity_command_pub = rospy.Publisher("/velocity_commands", Twist,  queue_size=10)
        # known issues, without sleep 1 second, publishers could not been setup
        rospy.sleep(1)
        # https://answers.ros.org/question/9665/test-for-when-a-rospy-publisher-become-available/?answer=14125#post-id-14125

        # enable pos_controller, velocity_controller by default
        self.enabled_pos_controller = True
        self.enabled_velocity_controller = True
        self.switch_pos_controller_pub.publish(Bool(True))
        self.switch_velocity_controller_pub.publish(Bool(True))