ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

no messages received in topic after transformation

asked 2021-07-28 07:56:55 -0500

offtaste gravatar image

updated 2021-08-04 14:24:22 -0500

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. <launch>

    <!--  ************** 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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

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

Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-07-28 08:13:47 -0500 )edit
1

In addition to @Ranjit's comment, the structure of an Odometry message differs from a PoseStamped. Take another look at that.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2021-07-28 15:42:40 -0500 )edit
Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-07-29 06:10:09 -0500 )edit

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.

offtaste gravatar image offtaste  ( 2021-07-29 06:48:08 -0500 )edit

That's nice! @offtaste, Don't forget to answer your question and make it tick. It can help others

Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-07-29 06:52:27 -0500 )edit

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!

offtaste gravatar image offtaste  ( 2021-07-31 06:17:34 -0500 )edit

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.

Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-08-03 07:54:13 -0500 )edit
1

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

offtaste gravatar image offtaste  ( 2021-08-04 14:06:45 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2021-08-05 12:01:00 -0500

Ranjit Kathiriya gravatar image

updated 2021-08-06 05:35:16 -0500

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

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/geo...

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.

edit flag offensive delete link more

Comments

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!

offtaste gravatar image offtaste  ( 2021-08-07 12:43:29 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2021-07-28 07:56:55 -0500

Seen: 242 times

Last updated: Aug 06 '21