Hello,
I am currently trying to use the navsat_transform_node from the robot_localization package in a ROS-Gazebo simulation. I'm using Ubuntu 16.04 LTS, with ROS kinetic and Gazebo 7.
In order to work, this node needs to subscribe to 3 differents topics : an odometry topic with nav_msgs/Odometry message, an IMU topic with sensor_msgs/Imu and a GPS topic with sensor_msgs/NavSatFix.
My problem is that, when launching the navsat_transform_node does not subscribe to my IMU topic, even if the message type of the IMU topic published by gazebo is sensor_msgs/Imu. However, the node does subscribe to the odom and gps topics.
Here is my launch file (taken from the robot localization wiki) :
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">
<param name="magnetic_declination_radians" value="0"/>
<param name="yaw_offset" value="0"/>
<remap from="/imu/data" to="/imu_data" />
<remap from="/gps/fix" to="/gps/fix" />
<remap from="/odometry/filtered" to="/odom" />
</node>
</launch>
Here is the IMU code from the URDF file of my robot model :
<joint name="imu_joint" type="fixed">
<axis xyz="0 0 1"/> <!-- 0 1 0 -->
<origin xyz="${length_chassis*(5/16)} 0 ${height_chassis-(wheel_radius/2)}"/>
<parent link="base_link"/>
<child link="imu_link"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=" ${IMU_size} ${IMU_size} ${IMU_size}"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${IMU_size} ${IMU_size} ${IMU_size}"/>
</geometry>
</collision>
</link>
And here is the IMU plugin :
<gazebo >
<plugin name="imu_plugin" filename="libhector_gazebo_ros_imu.so">
<alwaysOn>true</alwaysOn>
<bodyName>imu_link</bodyName>
<frameId>imu_link</frameId>
<topicName>/imu_data</topicName>
<serviceName>imu_service</serviceName>
<gaussianNoise>0.0</gaussianNoise>
<updateRate>20.0</updateRate>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
</plugin>
</gazebo>
When echo-ing the topic /imu_data, I get :
---
header:
seq: 327
stamp:
secs: 16
nsecs: 400000000
frame_id: "imu_link"
orientation:
x: 4.69962116857e-06
y: -4.8838604247e-08
z: 7.97720664175e-05
w: 0.999999996807
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: -4.49165569326e-13
y: 6.43295283435e-15
z: 2.52244385042e-12
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 3.40118210419e-07
y: 9.18946096526e-05
z: 9.79999999084
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
Also, @Tom Moore told me to post a sample message for every sensor input, so here is the sample from the topic /odom :
---
header:
seq: 165673
stamp:
secs: 1661
nsecs: 260000000
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 0.267229286097
y: 0.00580914153162
z: 0.299999489939
orientation:
x: 1.20893461539e-05
y: -8.60155713043e-08
z: 0.0124744407312
w: 0.999922191064
covariance: [1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
twist:
linear:
x: 0.000281084109386
y: -4.7415432993e-06
z: 0.0
angular:
x: 0.0
y: 0.0
z: 1.31254728036e-05
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
And here, the sample from the /gps/fix topic :
---
header:
seq: 7159
stamp:
secs: 1794
nsecs: 500000000
frame_id: "GPS_link"
status:
status: 0
service: 0
latitude: -30.0602189385
longitude: -51.1739172289
altitude: 9.7517203133
position_covariance: [0.0025010000000000006, 0.0, 0.0, 0.0, 0.0025010000000000006, 0.0, 0.0, 0.0, 0.0025010000000000006]
position_covariance_type: 2
---
If you have any ideas on how to fix this issue, I will be pleased ! Thank you
EDIT after @cagatayyuruk answer :
changed the plugin of my IMU from libgazebo_ros_imu.so to libhector_gazebo_ros_imu.so because the old one was not able to get imu_link as frame id. (code of the IMU and sample of the IMU's message are updated in the text above)
after a few test, it seems that the navsat node can only subscribe to the IMU topic when the TF_static between IMU_link and base_link is not set. So when all the tf static of the robot are launched, the navsat node does not work. Maybe I got something wrong in my TF that stop the subscribe of the navsat node to the IMU topic ?
EDIT for the TF Tree on Imgur : https://imgur.com/mVl7iCG
Originally posted by antonin_haulot on ROS Answers with karma: 215 on 2018-03-30
Post score: 3
Original comments
Comment by stevejp on 2018-04-03:
I'm a little suspicious that it has something to do with your TFs. If you could post your TF tree (without trying to run navsat transform) that would be useful.
Comment by antonin_haulot on 2018-04-04:
I can not post image since I do not have enough karma point to upload a file
Comment by l4ncelot on 2018-04-04:
Upload it somewhere else and post the link here.
