Ask Your Question
0

How to Subscribe to Imu data and broadcast transform

asked 2020-02-25 11:57:37 -0500

SamH gravatar image

I have a UAV application and I am using Hector-SLAM for the navigation. I have got Hector-SLAM working with just the map, base link and laser frames but I would like to add a base_stabilised frame to read data from the IMU and correctly reference the base link.

I have set up a static transform between the base_stabilised and base_link frames (as below) but I have not managed to vary this transform based on data from the IMU.

image description

The topic with the IMU data is: "mavros/imu/data" and the message type is "sensor_msgs/Imu".

I have got the stabilized_to_link.py file to this stage but I don't know how to actually subscribe to the Imu data and create a transform from it. Please can someone help? Many Thanks!

import roslib
import rospy
import time
import tf
from sensor_msgs.msg import Imu

def publisher_of_tf():
rospy.init_node('imu_data_broadcaster', anonymous=True)
rate = rospy.Rate(10.0) #10Hz

imu_data = rospy.Subscriber("/mavros/imu/data", Imu) #Subscribes to /mavros/imu/data topic

br = tf.TransformBroadcaster()

while not rospy.is_shutdown():
    br.sendTransform((0.0, 0.0, 0.0),
                     (0.0, 0.0, 0.0, 1.0),
                     rospy.Time.now(),
         #child
                     "base_link",
         #parent
                     "base_stabilized")
         rate.sleep()   

if __name__ == '__main__':
try:
    publisher_of_tf()
except rospy.ROSInterruptException:
    pass
edit retag flag offensive close merge delete

Comments

I think you need to add a callback to your subscriber.

MichaelDUEE gravatar image MichaelDUEE  ( 2020-02-25 13:41:20 -0500 )edit

Thanks for your suggestion. The file did originally contain a callback function and it prevented it from working which is why I took it out!

SamH gravatar image SamH  ( 2020-02-26 06:32:17 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2020-02-28 08:31:26 -0500

SamH gravatar image

Although I have not written a python code to implement this myself, I did find the hector_imu_attitude_to_tf package was able to achieve what I was looking to do. If anyone else is trying to do it then it's available here:

link text

I found that the Launch file needed modifying to remap the topics I was working with. I hope this helps someone else!

edit flag offensive delete link more
1

answered 2020-02-25 13:27:43 -0500

Without looking at the code, I think what you really want is the tree to be as it is, but remove the stabilized frame as a child of map.

If you have the stabilized frame there, then the normal base link frame has to go through a reduction in dimensionality and add it back in for 3D orientation.

Instead, add the stabilized frame as a child of the base link, so it now has 2 children: laser and stabilized. Then you can take the 3DOF pose in base link and then "stabilize" it as a frame of its own.

edit flag offensive delete link more

Comments

Thank you for your answer. What do you mean when you say that the base_link frame has to go through a reduction in dimensionality? I have set up the co-ordinate frames as exampled in the "How to set up hector_slam for your robot" page on the ROS Wiki website:

link text

If the data from the Lidar scanner doesn't go through the base_stabilized frame then how can it be corrected before it is mapped? Sorry for my ignorance!

SamH gravatar image SamH  ( 2020-02-26 05:03:16 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-02-25 11:57:37 -0500

Seen: 49 times

Last updated: Feb 28