Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

The problems are in the data of imu, you are fusing this data of sensor with gps, you need know if the imu data are corrected, I paint your rute and have the next image description green is the path filtered and rose is the position gps, i created a tools for visualization. The imu has follow the rep 103, try if the frame of imu is corrected, launch only the imu and test it.

I changed your .launch

<launch>

    <node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="--clock /home/jorge_j/catkin_ulises/src/pruebas/covar_2015-10-05-10-02-06.bag -d 3"/>
    <!-- <node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="clock /home/Downloads/filtered.bag -d 3"/> -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 base_link imu_link" />


 <param name="odom0_queue_size" value="10"/> -->

    <!-- Global (map) instance -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global" clear_params="true">
      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <!-- <param name="odom0" value="/jackal_velocity_controller/odom"/> -->
      <param name="odom0" value="/odometry/gps"/>
      <param name="imu0" value="/imu/data"/>

      <!-- <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true, true, true,
                                      false, false, true,
                                      false, false, false]</rosparam> -->

      <rosparam param="odom0_config">[true, true, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam param="imu0_config">[false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true]</rosparam>

      <!-- <param name="odom0_differential" value="false"/> -->
      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <!-- <param name="odom0_relative" value="false"/> -->
      <param name="odom0_relative" value="false"/>
      <param name="imu0_relative" value="false"/>

      <param name="imu0_remove_gravitational_acceleration" value="true"/>
      <param name="print_diagnostics" value="true"/>

      <!-- <param name="odom0_queue_size" value="10"/> -->
      <param name="odom0_queue_size" value="10"/>
      <param name="imu0_queue_size" value="10"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
   </node>

   <!-- navsat_transform -->
   <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
      <param name="frequency" value="30"/>
      <param name="delay" value="3"/>
      <param name="magnetic_declination_radians" value="0.190240888"/>
      <!-- <param name="magnetic_declination_radians" value="-0.161617489"/> -->
      <!-- <param name="yaw_offset" value="1.570796327"/> -->
      <param name="yaw_offset" value="1.570796327"/>
      <param name="zero_altitude" value="false"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
      <param name="use_odometry_yaw" value="true"/>
      <param name="wait_for_datum" value="false"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
      <!-- <remap from="/gps/fix" to="/navsat/fix"/> -->
    </node>

</launch>

The problems are in the data of imu, you are fusing this data of sensor with gps, you need know if the imu data are corrected, I paint your rute and have the next image description green is the path filtered and rose is the position gps, i created a tools for visualization. The imu has follow the rep 103, try if the frame of imu is corrected, launch only the imu and test it.it. The x axis of the base_link frame should point in the direction of advance of the robot

I changed your .launch

<launch>

    <node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="--clock /home/jorge_j/catkin_ulises/src/pruebas/covar_2015-10-05-10-02-06.bag -d 3"/>
    <!-- <node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="clock /home/Downloads/filtered.bag -d 3"/> -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 base_link imu_link" />


 <param name="odom0_queue_size" value="10"/> -->

    <!-- Global (map) instance -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global" clear_params="true">
      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <!-- <param name="odom0" value="/jackal_velocity_controller/odom"/> -->
      <param name="odom0" value="/odometry/gps"/>
      <param name="imu0" value="/imu/data"/>

      <!-- <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true, true, true,
                                      false, false, true,
                                      false, false, false]</rosparam> -->

      <rosparam param="odom0_config">[true, true, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam param="imu0_config">[false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true]</rosparam>

      <!-- <param name="odom0_differential" value="false"/> -->
      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <!-- <param name="odom0_relative" value="false"/> -->
      <param name="odom0_relative" value="false"/>
      <param name="imu0_relative" value="false"/>

      <param name="imu0_remove_gravitational_acceleration" value="true"/>
      <param name="print_diagnostics" value="true"/>

      <!-- <param name="odom0_queue_size" value="10"/> -->
      <param name="odom0_queue_size" value="10"/>
      <param name="imu0_queue_size" value="10"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
   </node>

   <!-- navsat_transform -->
   <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
      <param name="frequency" value="30"/>
      <param name="delay" value="3"/>
      <param name="magnetic_declination_radians" value="0.190240888"/>
      <!-- <param name="magnetic_declination_radians" value="-0.161617489"/> -->
      <!-- <param name="yaw_offset" value="1.570796327"/> -->
      <param name="yaw_offset" value="1.570796327"/>
      <param name="zero_altitude" value="false"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
      <param name="use_odometry_yaw" value="true"/>
      <param name="wait_for_datum" value="false"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
      <!-- <remap from="/gps/fix" to="/navsat/fix"/> -->
    </node>

</launch>

The problems are in the data of imu, you are fusing this data of sensor with gps, you need know if the imu data are corrected, I paint your rute and have the next image description green is the path filtered and rose is the position gps, i created a tools for visualization. The imu has follow the rep 103, try if the frame of imu is corrected, launch only the imu and test it. The x axis of the base_link frame should point in the direction of advance of the robotrobot. The angular_velocity and lineal acceleration of imu not is consistent with vehicle movement

I changed your .launch

<launch>

    <node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="--clock /home/jorge_j/catkin_ulises/src/pruebas/covar_2015-10-05-10-02-06.bag -d 3"/>
    <!-- <node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="clock /home/Downloads/filtered.bag -d 3"/> -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 base_link imu_link" />


 <param name="odom0_queue_size" value="10"/> -->

    <!-- Global (map) instance -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global" clear_params="true">
      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="true"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <!-- <param name="odom0" value="/jackal_velocity_controller/odom"/> -->
      <param name="odom0" value="/odometry/gps"/>
      <param name="imu0" value="/imu/data"/>

      <!-- <rosparam param="odom0_config">[false, false, false,
                                      false, false, false,
                                      true, true, true,
                                      false, false, true,
                                      false, false, false]</rosparam> -->

      <rosparam param="odom0_config">[true, true, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam param="imu0_config">[false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true]</rosparam>

      <!-- <param name="odom0_differential" value="false"/> -->
      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

      <!-- <param name="odom0_relative" value="false"/> -->
      <param name="odom0_relative" value="false"/>
      <param name="imu0_relative" value="false"/>

      <param name="imu0_remove_gravitational_acceleration" value="true"/>
      <param name="print_diagnostics" value="true"/>

      <!-- <param name="odom0_queue_size" value="10"/> -->
      <param name="odom0_queue_size" value="10"/>
      <param name="imu0_queue_size" value="10"/>

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug_ekf_localization.txt"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
   </node>

   <!-- navsat_transform -->
   <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
      <param name="frequency" value="30"/>
      <param name="delay" value="3"/>
      <param name="magnetic_declination_radians" value="0.190240888"/>
      <!-- <param name="magnetic_declination_radians" value="-0.161617489"/> -->
      <!-- <param name="yaw_offset" value="1.570796327"/> -->
      <param name="yaw_offset" value="1.570796327"/>
      <param name="zero_altitude" value="false"/>
      <param name="broadcast_utm_transform" value="true"/>
      <param name="publish_filtered_gps" value="true"/>
      <param name="use_odometry_yaw" value="true"/>
      <param name="wait_for_datum" value="false"/>

      <remap from="/odometry/filtered" to="/odometry/filtered/global"/>
      <!-- <remap from="/gps/fix" to="/navsat/fix"/> -->
    </node>

</launch>