0
$\begingroup$

Rosanswers logo

Hello,

we are driving a vehicle with GPS and automated path planning and I recorded the drives. The rosbag is playing great, but I wanted to visualize the driven path to check the accuracy.

I tried something else but then thought it would work good enough with tracking odometry in RVIZ. I thought using the gps signal would be the best and I have a topic /odometry/gps which is a PoseStamped. To visualize it in RVIZ, I wanted to transform it into a /odometry message, which I could then show in RVIZ. I use this script for it:

import rospy
from geometry_msgs.msg import PoseStamped, Quaternion
from nav_msgs.msg import Odometry
from tf.transformations import quaternion_from_euler, euler_from_quaternion


class OdomPub:
def __init__(self):
        self.odom = Odometry()
        self.odometry_pub = rospy.Publisher('/odometry', Odometry, queue_size=1)
    rospy.Subscriber("/odometry/gps", PoseStamped, self.pose_callback)

      def rotate_quaternion(self):
            # TS 2021_07_26: the heading is measured from antenna 1 (front) to antenna 2 (back) and thus mirrored over origin 
            quat = [self.odom.pose.orientation.x, self.odom.pose.orientation.y, self.odom.pose.orientation.z, self.odom.pose.orientation.w]
            (alpha,beta,gamma) = euler_from_quaternion(quat, axes='sxyz')
        
            self.odom.orientation.quaternion = Quaternion(*quaternion_from_euler(alpha, beta, -gamma))
        
        def publish_heading(self):
            self.odometry_pub.publish(self.odom)
            
        def pose_callback (self, pose):
            self.odom.header = pose.header
            self.odom.pose = pose.pose
            self.odom.child_frame_id = "base_link"

            #self.rotate_quaternion()  

if __name__ == '__main__':
        #print('something started')
        rospy.init_node('trajectory_pub')
        OdomPub()
        rospy.spin() here

Here is my problem: I get the topic published with no warnings left, BUT when I echo the /odometry, I get the followiing

 rostopic echo /odometry 
    WARNING: no messages received and simulated time  is active. 
    Is /clock being published?

of course /clock is active, because i play rosbag with clock.

So again: on /odometry/gps (which is the PoseStamped) I get a good gps signal, but still not on the /odometry (Odometry) topic.

edit: I added some lines to call the publish_heading and I get no errors, but still doesn't work.

if name == 'main': print('something started') odom_pub=OdomPub() rospy.init_node('trajectory_pub') odom_pub.publish_heading() OdomPub() rospy.spin()

Here is the launch file.

    <!--  ************** Global Parameters ***************  -->
  <param name="/use_sim_time" value="true"/>


    <arg name="target_x_vel" default="2.3"/>
    <arg name="target_yaw_vel" default="0.8"/>
    <arg name="robot_radius" default="0.6"/>
    <arg name="tool_radius" default="0.6"/>   

  <node pkg="tf" type="static_transform_publisher" name="odom_tf_publisher" args="0 0 0 0 0 0 1 map odom 100" />
  <node pkg="tf" type="static_transform_publisher" name="utm_to_map_tf_publisher" args="361282.288 5814827.341 0 0 0 1 utm map 100" />
  <node name="OdomPub" pkg="gps_localization" type="PoseStamped_to_odometry.py"/>


    <!--  ************** Navigation ***************  -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

  <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" command="load" />
****** Maps *****
   ******* Visualisation ****** 
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation.rviz"/>

Are there faults in my scripts? Thanks in advance!!!


Originally posted by offtaste on ROS Answers with karma: 13 on 2021-07-28

Post score: 0


Original comments

Comment by Ranjit Kathiriya on 2021-07-28:
Can you please! tell me where are you calling the publish_heading() method? Is it a half code?

If no, then you have to call publish_heading() for publishing self.odom

Comment by Mike Scheutzow on 2021-07-28:
In addition to @Ranjit's comment, the structure of an Odometry message differs from a PoseStamped. Take another look at that.

Comment by Ranjit Kathiriya on 2021-07-29:
Thanks, @Mike Scheutzow

Comment by offtaste on 2021-07-29:
Thanks, @Ranjit , I had that script for another transformation and just tried to rebuild it for my purpose without really understanding all the relations. I will try to add it and come back.

@Mike I recognized that, but I think I gave everything the odometry needs in the callback? (Header, pose, childframeid) There is just a velocity missing, but there is none so it should be set to zero, which would be fine for me.

Comment by Ranjit Kathiriya on 2021-07-29:
That's nice! @offtaste, Don't forget to answer your question and make it tick. It can help others

Comment by offtaste on 2021-07-31:
Hey @Ranjit, I looked for the call and tried to do it in the last sector. Is it enough just to call publish_heading() ? There occured an error, so i arranged it like with no error but still no data on /odometry:

if __name__ == '__main__':
#print('something started')
odom_pub=OdomPub()
rospy.init_node('trajectory_pub')
odom_pub.publish_heading()
OdomPub()
rospy.spin()

And i remembered why I did not cll it, because I thought calling OdomPub() would start everything.

Could you give me an advice about calling it right? Thanks a lot!

Comment by Ranjit Kathiriya on 2021-08-03:
Apologies for the late reply, I was on my holiday.

Can you please! upload the full code in your question section? You can do so by clicking on the edit button just below your question.

If you are getting any errors, include that also. Your details are important for us to get clues.

Comment by offtaste on 2021-08-04:
Hey @Ranjit, please dont apologize for holidays! Your help is great even if it takes time. The code above IS everything I have inside that .py!!! Otherwise I would have already added it. I will add the launch part, but I think it's not that helpful? I will look after errors and maybe add them, but actually I dont know any...

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hello @offtaste,

I have checked this issue, and I think that this is the correct solution.

  1. As @Mike Scheutzow told that there is some difference in Odometry and PoseStamped, Yes, the pose name is the same for both but the inside definition (message) content is different.

a. nav_msgs/Odometry Message > pose has: geometry_msgs/PoseWithCovariance pose

geometry_msgs/Pose pose
float64[36] covariance

Ref: http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html

b. geometry_msgs/PoseStamped Message > pose has: geometry_msgs/Pose pose

geometry_msgs/Point position
geometry_msgs/Quaternion orientation

Ref: http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html

This is an easy fix where you are using self.odom.pose instead of this make it self.odom.pose.pose, I think it will work.

I am attaching the code below, I think it will help you to identify the issue.

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped, Quaternion
from nav_msgs.msg import Odometry
from tf.transformations import quaternion_from_euler, euler_from_quaternion


class OdomPub:
    def __init__(self):
        self.odom = Odometry()
        self.odometry_pub = rospy.Publisher('/odometry', Odometry, queue_size=1)
        rospy.Subscriber("/odometry/gps", PoseStamped, self.pose_callback)

    def publish_heading(self):
        self.odometry_pub.publish(self.odom)

    def pose_callback (self, pose):
        print(pose)
        self.odom.header = pose.header
        self.odom.pose.pose = pose.pose  # Changed over here self.odom.pose.pose
        self.odom.child_frame_id = "base_link"
        self.publish_heading()

        #self.rotate_quaternion()

    def rotate_quaternion(self):
        # TS 2021_07_26: the heading is measured from antenna 1 (front) to antenna 2 (back) and thus mirrored over origin
        quat = [self.odom.pose.orientation.x, self.odom.pose.orientation.y, self.odom.pose.orientation.z, self.odom.pose.orientation.w]
        (alpha,beta,gamma) = euler_from_quaternion(quat, axes='sxyz')

        self.odom.orientation.quaternion = Quaternion(*quaternion_from_euler(alpha, beta, -gamma))

if __name__ == '__main__':
        #print('something started')
        rospy.init_node('trajectory_pub')
        OdomPub()
        rospy.spin()

if your data is unique in self.rotate_quaternion() function then ignore this point. Else, I am not sure about your application, but just to note, now you don't need to call self.rotate_quaternion() this function because now pose has both geometry_msgs/Point position and geometry_msgs/Quaternion orientation in your odometry.

I think it will work. I have tested myself. If you are facing any issues feel free to drop a comment.


Originally posted by Ranjit Kathiriya with karma: 1622 on 2021-08-05

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by offtaste on 2021-08-07:
hey @Ranjit, I don't know what to say. THANK YOU so! much. I receive a wonderful odometry now which I can track in RVIZ. So it seems to be just that "double .pose" and a good heading call. The rotate quaternion is another topic I could now turn to, because that pose/odom topic is 180 degree switched to my robots orientation. That's what that part ist for, to then have the odometry pointing into the right direction.

(I will check that answer as right. Is that enough or do I need to edit my post again?) Again: Thanks! You're the best!

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.