Robot localization - With IMU stable; adding other pose sources(eg. AMCL) causes seizures.
On the quest to stop the robot from drifting we have implemented an IMU which makes use of a madgwick filter. After painstaking troubleshooting with both the arduino and the raspberry pi used to read and convert the data for the ROS core to use, we've gotten it to work and display somewhat stable within rviz, until we started driving the robot. The main issue, which has been resoled is that on rotation, the robot within rviz starts shaking which gets heavier the more we rotate it, yet when we return to its original position the shaking would stop but the position is rotated from its original setting.
The remaining issue is that when adding the laser_scan_matcher's position to the robot_localization mix would cause the robot within rviz to warp to and fro at a high rate, incrementing wrong/concurrent position data until the robot's off the map completely. Adding amcl's postion data causes an epileptic seizure to both rviz and the robot's positioning data, luckily not to ourselves.
The IMU has been properly reconfigured as per the last edit(s), however I can't seem to find the cause for the concurrency between the different posing algorithms nor can I discover the right method of implementing one or two robot_localization nodes as suggested in some other answers found on the community.
In short, the question would be how I would implement both laser_scan_matcher for positioning via LIDAR (original method) and/or amcl without them and the IMU fighting against each other?
The necessary configuration files are as follows, along with the frames.pdf that tf provided:
navigation.launch: <launch> <arg name="sim" default="false"/> <arg name="path"/>
<node if="$(eval sim == 'true')" pkg="rosbag" type="play" name="play" args="$(arg path)/bags/t5_lidar.bag --delay=5 --clock"/>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.15 0.0 0.0 0.0 0.0 0.0 /base_link /laser 40" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_imu" args="-0.12 0.23 0.0 0.0 0.0 0.0 /base_link /imu_base 40" />
<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0 0 0.0 0.0 0.0 /world /map 40"/>
<node pkg="tf" type="static_transform_publisher" name="world_to_map_nav" args="0.0 0 0 0.0 0.0 0.0 /world /map_nav 40"/>
<!--
<node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
<rosparam command="load" file="$(arg path)/config/laser_filter_config.yaml" />
</node>
-->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(arg path)/navigation.rviz"/>
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">
<rosparam file="$(arg path)/config/laser_scan_matcher_params.yaml" command="load" />
<remap from="scan" to="scan_filtered" />
</node>
<node pkg="map_server" type="map_server" name="map_server" args="$(arg path)/maps/T5_full_small.yaml" output="screen"/>
<node pkg="map_server" type="map_server" name="map_server_nav" args="$(arg path)/maps/T5_full_small_move_base.yaml" output="screen">
<remap from="map" to="map_nav"/>
</node>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom" clear_params="true">
<rosparam command="load" file="$(arg path)/config/robot_localization_params.yaml" />
</node ...
so you still need to do this?
Many packages have an (implicit) assumption (based on REP-103) about chirality and axis alignment. If your data doesn't adhere to that, things will go wrong.
We're doing this right now, considering the rotation and linear acceleration is reversed; our foolish mistake. Edit; we've resolved the IMU's goofed axes, thereby eliminating the epileptic rotations. However, the map now skews to the right/down a little bit. A problem from the past come to haunt us again.
In a round of experimentation, I've found that adding a secondary ekf node in place of
amcl(ergo map->odom) would not cause any epileptic effects, but when moving the robot: it does move accurately, but the map is frozen in place. Can't discover why, and at worst it would make it impossible to use the "2D Pose Estimate" button withinrviz....Tis a monster we're working with, sadly.
Please include sample messages from each sensor input. Your EKF config is also very confusing, as you have a lot of things commented out. Off the top of my head, one thing I see straight away is that you are (or were?) fusing absolute pose data from a bunch of different sources.
Anyway, clear out anything in the EKF config that doesn't need to be there, and let's start with a simple setup, and work our way up. You clearly have some issue with conflicting frames, and/or multiple nodes trying to publish the same transform. But I'd need to see all the sensor inputs to comment further.
I was indeed fusing the estimated pose from
amcl(really bad idea.),laser_scan_matcherand the IMU at once. Since then I've reduced the set-up to what it was beforehand, hooking the IMU sensor tolaser_scan_matcherinstead.The sensor messages will be included asap, I happen to have noticed that the orientation of the laser and imu don't quite match either, so we're working on reducing that difference the best we can.
I also note that you have both the laser scan matcher and the EKF publishing to tf. I'm assuming that means you have two odom->base_link publishers. That's going to make lots of things unhappy.
Learned that the hard way, so as a result I've set
publish_tfto false in the laser scan matcher to have EKF be the only one. One gripe is that I was also feeding the position estimate from the laser scan matcher into EKF at a point. In later experiments I've found that the laser was often publishing position estimates that would be accurate (around 0.02) but often jump to values higher than 5. How this is caused is unknown.So is this all working for you now?