robot_pose_ekf frequency too slow? [closed]
Hello everybody,
I am currently trying to modify the Husky robot by adding an IMU to it and use the robot_pose_ekf to get a pose of the robot by combining the input of the wheel odometry and the IMU. I do hope, that this is not a copy of this (answers.ros.org/question/12562/is-robot_pose_ekf-acting-correctly/, not enough karma for links), as I do not get any warnings (frame_id of the IMU is set to base_footprint).
Things are working fine so far, although the update rate of the robot_pose_ekf is very slow, even if all relevant frequencies are set to 30 Hz or more. Currently robot_pose_ekf publishes only with a rate of 0.2 Hz, although it should report with a rate of 30 Hz. For example rosservice call /robot1/robot_pose_ekf/get_status will yield the following:
status: Input:
* Odometry sensor
- is active
- received 1227 messages
- listens to topic /robot1/odom
* IMU sensor
- is NOT active
- received 1343 messages
- listens to topic /robot1/imu_data
* Visual Odometry sensor
- is NOT active
- received 0 messages
- listens to topic
Output:
* Robot pose ekf filter
- is active
- sent 47 messages
- pulishes on topics /robot1/robot_pose_ekf/robot1_tf/odom and /tf
The code I am using to include the robot_pose_ekf is the following:
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="freq" value="30.0" />
<param name="sensor_timeout" value="1.0" />
<param name="odom_used" value="true" />
<param name="imu_used" value="true" />
<param name="vo_used" value="false" />
<param name="output_frame" value="odom" />
</node>
I would be delighted, if any of you could shed any light on my problem and help me to get a decent publishing frequency for the robot_pose_ekf topic. Thanks in advance for any answers.
Without looking at this closely, the "is NOT active" line in "IMU sensor" strikes me as odd.
Thanks for the answer, any idea on how to activate the IMU sensor? "rostopic info /robot1/imu_data" shows, that the ekf subscribes to the imu topic.
Unfortunately not. You might want to dig in the code to find out why IMU is shown as not active. Also what frequency is your odom input?
If I haven't made an error looking into the Husky model, the odom frequency should be set to 100. As far as I could discern from the robot_pose_ekf package, the pose will only update if it gets two different inputs with a later timestamp, but IMHO this should not matter in this case.
you can easily check with `rostopic hz /robot1/odom`
"rostopic hz..." tells me, that odom has a rate around 800, should be fast enough Tod allow the robot_pose_ekf to update with a rate of 30 Hz. Maybe I just have Tod take a peek at the ekf source code, but thanks either way.
Also, maybe add a `output="screen"` to your launch file, to see if there is any output of the ekf node that you might be missing.
Thanks for the hint, I did not know about that parameter yet. I will try it out and check whether ekf has anything more to say ;)