Reputation: 11
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)
Upvotes: -1
Views: 190
Reputation: 11
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.
Upvotes: 0