rviz not updating imu pose

asked 2019-12-08 16:06:54 -0500

nikisalli
15 ●2 ●3 ●5

updated 2022-04-11 14:44:29 -0500

lucasw

8729 ●137 ●239 ●256 https://github.com/lucasw

platform: lattepanda alpha d864 OS: ubuntu 18.04 kernel:4.15-72 distro: ros melodic

I wrote this simple code to read RPY data from the imu of my robot and it works:

def publish_imu(self):
    self.imu_msg = Imu()

    self.imu_msg.orientation_covariance = [0.01 ,0    ,0
                                          ,0    ,0.01 ,0
                                          ,0    ,0    ,0.01]

    quaternion = tf.transformations.quaternion_from_euler(imu_listener.roll*self.degrees2rad,
                                                          imu_listener.pitch*self.degrees2rad,
                                                          imu_listener.yaw*self.degrees2rad)

    self.imu_msg.orientation.x = quaternion[0] # x
    self.imu_msg.orientation.y = quaternion[1] # y
    self.imu_msg.orientation.z = quaternion[2] # z
    self.imu_msg.orientation.w = quaternion[3] # w

    self.imu_msg.header.stamp = rospy.Time.now()
    self.imu_msg.header.frame_id = self.frame_name
    self.imu_msg.header.seq = self.seq

    self.pub_imu.publish(self.imu_msg)
    self.seq += 1

in fact if I run rostopic list I get:

/diagnostics_agg
/nightmare/imu
/rosout
/rosout_agg
/statistics
/tf

and if I run rostopic echo nightmare/imu I get all the messages being sent correctly:

---
header: 
  seq: 89296
  stamp: 
    secs: 1575842102
    nsecs: 509346961
  frame_id: "imu"
orientation: 
  x: 0.009329143931687798
  y: 0.029986258160736832
  z: -0.47810897774189304
  w: 0.8777389115207446
orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
angular_velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

this is the launch file I'm using:

<launch>
    <node pkg="nightmare" name="listener" type="listener.py" output="screen">
        <param name="fixed_frame" type="string" value="map" />
        <param name="publish_transform" type="bool" value="true" />
    </node>
</launch>

but when I run rviz I can't get the pose axes to move accordingly to the imu and the imu frame just stays overlapped on the map frame no matter what I do: https://ibb.co/dGfxKdv

what am I doing wrong? why is the imu frame not moving accordingly with the real imu?

edit retag flag offensive close merge delete

add a comment

answered 2019-12-09 05:47:39 -0500

Reamees
591 ●13 ●11

updated 2019-12-09 05:48:28 -0500

Can not tell for sure from the image and code posted, but you seem to be trying to visualize the transform between imu and map frames in rviz. You seem to have some transform between the two, but that part of your code is not posted here, to make it work like you have it now you would have to use the "quaternion" you get from the imu to calculate the transform and publish it.

For debugging, that would be fine, I guess. In the end you probably don't want your imu node to publish this transform, it's probably not the only sensor you will use for your localization and it would get quite messy if every sensor is connected to some base_link and is publishing it's own transform. So you will probably use some node that will take in all the sensors and calculate the transform based on all of them. One such node is the robot_localization package.

If you want to visualize the heading of your imu in rviz, there are several options. You could install the rviz imu plugin and use that. Or you could publish some message alongside the IMU message that are easier to visualize in Rviz. Like for example the PoseStamped from geometry_msgs. Just define a pose_msg the same way you did imu_msg and a publisher for pub_pose similar to pub_imu and add the following lines to the end of publish_imu function

self.pose_msg.header = self.imu_msg.header
self.pose_msg.pose.orientation = self.imu_msg.orientation
self.pub_pose.publish(self.pose_msg)

Then you should be able to add the pose visualization to rviz. Open rviz, click the "Add" option in the "displays" panel, switch to the "By topic" tab and add the pose message to the displays.

Also you probably should remove the sequence counting and setting from your node. Ros will handle counting and setting the sequence on its own. One relevant ros answers question regarding that.

Comments

it works now thanks! I used rviz imu plugin and by setting imu as fixed frame I can now see the three axes move correctly

nikisalli ( 2019-12-09 08:50:01 -0500 )edit

add a comment

rviz not updating imu pose

1 Answer

Comments

Question Tools

Stats

Related questions

rviz not updating imu pose edit

1 Answer

Comments

Question Tools

Stats

Related questions

rviz not updating imu pose