GPS + IMU Fusion using robot-pose-ekf not working

asked 2021-10-20 11:30:51 -0500

aditya_c2512 gravatar image

I'm working on a UGV and I have an IMU publisher publishing to /imu_data, a GPS publishing to /gps_meas. I followed the WikiROS on robot-pose-ekf to add GPS but there's no output from the robot-pose-ekf node. What could the problem be? Here's the launch file :

<launch>

<!-- Transformation Configuration ... Setting Up the Relationships Between Coordinate Frames -->
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.06 0 0.08 0 0 0 base_link laser 30" />
<node pkg="tf" type="static_transform_publisher" name="imu_broadcaster" args="0 0.06 0.02 0 0 0 base_link imu 30" />
<node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0.09 0 0 0 base_footprint base_link 30" />
<!-- odom to base_footprint transform will be provided by the robot_pose_ekf node -->

<!-- IMU Data Publisher Using the MPU9250 IMU Sensor -->
<!-- Publish: /imu/data -->
<node pkg="imu" name="mpu9250_node" type="mpu9250_node" respawn="true" respawn_delay="2">
    <param name="device" type="string" value="/dev/i2c-1"/>
    <param name="address" type="int" value="40"/>
    <param name="frame_id" type="string" value="imu"/>
</node>

<!-- GPS Data Publisher Using the NEO6M GPS Modeule -->
<!-- Publish : /gps_meas -->
<node pkg="gps" name="gps_node" type="gps_node" respawn="true" respawn_delay="2">
</node>


<!-- Convert GPS Data to Odometry using utm_odometry_node -->
<node name="gps_conv" pkg="gps_common" type="utm_odometry_node">
    <remap from="odom" to="vo"/>
    <remap from="fix" to="/gps/fix" />
    <param name="rot_covariance" value="99999" />
    <param name="frame_id" value="base_footprint" />
</node>

<!-- Extended Kalman Filter from robot_pose_ekf Node-->
<!-- Subscribe:/imu_data -->
<!-- Publish: /robot_pose_ekf/odom_combined -->
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <remap from="vo" to="gps_meas" />
    <param name="output_frame" value="odom"/>
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="false"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="gps_used" value="true"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
</node>

<!-- Controller Node -->
<node pkg="controller" name="controller_node" type="controller_node" respawn="true" respawn_delay="2">
</node>

<node pkg="hw_interface" name="hw_interface_node" type="hw_interface_node" respawn="true" respawn_delay="2">
</node>

</launch>

edit retag flag offensive close merge delete