Navsat_transform_node does not subscribe to IMU topic
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 ...
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.
I can not post image since I do not have enough karma point to upload a file
Upload it somewhere else and post the link here.