no messages received in topic after transformation
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 ...
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 publishingself.odom
In addition to @Ranjit's comment, the structure of an Odometry message differs from a PoseStamped. Take another look at that.
Thanks, @Mike Scheutzow
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.
That's nice! @offtaste, Don't forget to answer your question and make it tick. It can help others
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:
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!
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.
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...