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)
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)
the output can be given to q = quaternion_from_euler(ROLL,PITCH,YAW) in the code above.