How do i convert speed vector into Rall-Pitch-Yaw form to create transform?

135 views Asked by At

In Webots sim i have GPS (https://cyberbotics.com/doc/reference/gps?tab-language=ros) measurements with 2 topics: coords as Point and speed_vector as Vector3 (returns the current GPS speed vector in meters per second). So, I need to create TransformStamped from them. How do I convert the speed vector into RPY form? The code in ROS2 is

def __gps_sensor_callback(self, message):   
   t = TransformStamped()
   t.header.stamp = self.get_clock().now().to_msg()
   t.header.frame_id = 'world'
   t.child_frame_id = "My_robot"

   t.transform.translation.x = message.point.x
   t.transform.translation.y = message.point.y
   t.transform.translation.z = message.point.z

   # for the translation now I should use RPY, 
   # but I have a speed vector self.__gps_sv from another topic

   q = quaternion_from_euler(**ROLL**,**PITCH**,**YAW**)
   t.transform.rotation.x = q[0]
   t.transform.rotation.y = q[1]
   t.transform.rotation.z = q[2]
   t.transform.rotation.w = q[3]

   self.tf_broadcaster.sendTransform(t)
1

There are 1 answers

0
Dan On

For those, who stumbled with this thing... The question was how to create a Pose for the robot from GPS sensor given feedbacks from topics Point(x,y,z) and Speed_vector (x,y,z) with Webots sim.

The RPY from speed vector in 2D(only Yaw): v3 input is from the vector speed Vector3 (x ,y ,z) (v3 in this case)

def getRPY(v3):
    rad90=math.radians(90.0)
    done = False

    if v3.x == 0 and v3.y > 0.0: 
        rad = rad90
    if v3.x == 0 and v3.y < 0.0: 
        rad = rad90 * 3 # 270  
    if v3.x > 0.0 and v3.y == 0: 
        rad = 0
    if v3.x < 0.0 and v3.y == 0: 
        rad = rad90 * 2 # 180 

    if v3.x == 0 and v3.y == 0: 
        rad = 0
    if v3.x == 0 or v3.y == 0: 
        done = True  

   

    if not done: 
        tg = abs(v3.y/v3.x)
        
        rad = math.atan(tg)
        pi = math.pi
        
       
        if v3.x < 0 and v3.y > 0:
            rad = pi - rad # 180 - rad
        
        if v3.x < 0 and v3.y < 0:
            rad = pi + rad # 180 + rad
        
        if v3.x > 0 and v3.y < 0:
            rad = 2 * pi - rad
        
        
        
    yaw = rad   
    roll = 0
    pitch = 0

    return roll, pitch, yaw

the output can be given to q = quaternion_from_euler(ROLL,PITCH,YAW) in the code above.