I looked at the data from your other post, and this is what navsat_transform_node is outputting:
You can get this plot by doing:
rostopic echo -p /odometry/gps > odom_gps.txt
...and then plotting the output with MATLAB.
This says to me that navsat_transform_node
is behaving correctly, but keep reading.
Without doing a deeper analysis, my first guess is that your IMU may not be mounted correctly. If, for example, you have it turned around and facing the wrong direction, then navsat_transform_node
will be outputting incorrect positions if you don't correct for the offset, and ekf_localization_node
will think your robot is facing the wrong way, so when the EKF makes a prediction, it will move your robot the wrong way.
Please do this: do not remove any information from your question. Instead, at the bottom, add an "EDIT" section, and post a photo, if possible, of the IMU on your robot, and please make it clear where the front of your robot is. Also, although I believe I am aware of it, let me know if you're using this fork of the GX2 node, or the default one, as it will affect my answer.
EDIT 1:
Here's what I did:
- Downloaded your miercoles.bag file
Ran this command (to get rid of the topics I didn't want):
rosbag filter miercoles.bag gps_test.bag "topic != '/diagnostics' and topic != '/odometry/filtered' and topic != '/rosout' and topic != '/rosout_agg' and topic != '/tf' and topic != '/odometry/gps'"
- Downloaded the latest source for
robot_localization
, and used the indigo-devel branch - Created this launch file:
<launch>
<param name="/use_sim_time" value="true" />
<node pkg="tf" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 1 /base_link /imu 20" />
<node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/tmoore/Desktop/gps_test.bag -d 3"/>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" 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="odom0" value="odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<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="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="false"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="2"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="/home/tmoore/Desktop/debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[0.05, 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.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0 ...
(more)
Please post a picture of your IMU mounted on your robot. I want to make sure it's oriented correctly.
How did you plot your data? What did you use?
You can stack the position so rostopic echo -p /odometry/gps > odom_gps.csv and then plot a graphic with LibreOffice calc, other option is create a specific plugin for rviz, i did this plug-in that plot a odometry msg like a path msg. If you want this plug-in i can send you a link.
Can you send me the link for the plug-in. Thanks
https://github.com/porti77/rviz_plugi... I hope I've helped
Yeah,it helped me, thanks so much
hi, @Porti77, odometry msg path plot, as I know the rviz can do it already, just set the buffer up to 5000, it can plot the path. is this what you want? but gps path which rviz cannot plot.