The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
| | 2 | No.2 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources (and my money is on your IMU) is producing headings that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
| | 3 | No.3 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources (and my money is on your IMU) is producing headings that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
| | 4 | No.4 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
| | 5 | No.5 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
| | 6 | No.6 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources (and my money is on your IMU) is producing headings that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
| | 7 | No.7 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
| | 8 | No.8 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
| | 9 | No.9 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources (and my money is on your IMU) is producing headings that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
| | 10 | No.10 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources (and my money is on your IMU) is producing headings that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
| | 11 | No.11 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources (and my money is on your IMU) is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
| | 12 | No.12 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
First, here is the
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
| | 13 | No.13 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
First, here is the
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
| | 14 | No.14 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Keep reading, though, as there are some things you need to look into.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
Now, onto the issues:
First, your IMU timestamps are about 0.03 seconds in the future from all other times. On my machine, this causes the tf message filters I'm using to effectively ignore all of your IMU data. I'm guessing this is happening to you as well, but you can double-check by turning debug on and looking for imuCallback. You'll then see something like this:
WARNING: failed to transform from summit_a/imu->summit_a/base_footprint for imu0 message received at 1418218635.236272155.
I verified that the transforms were, in fact, being broadcast. Double-check what the IMU node is doing, and make sure its timestamps are correct. Does the IMU have an internal clock? How is it setting the timestamp value?
In the rviz output above, do you see how the red value (which was ekf_localization_node's output in the bag file you sent me) keeps going? That's because it's using the raw yaw velocity data. That would normally be fine, except for this:
That's a Matlab plot of your raw odometry measurement yaw velocity after I accumulated it (red), and the absolute measurement (blue). See how the absolute measurement planes out before the velocity? It seems as if the yaw velocity data is lagged behind the absolute yaw data. Again, these are raw measurements, and have not been filtered (at least not on my end; I have no idea what the Summit driver is doing behind the scenes). At first I thought that my filter was trusting its own yaw velocity estimate too much, and was taking too long to fuse the raw measurements, but then I plotted this:
Those values are the raw measurement yaw velocity (from the odometry message) and the filtered values from ekf_localization_node. As you can see, they are directly in sync, meaning that the filter is accepting each measurement with a high degree of confidence, and is moving the filtered yaw velocity output so that it is more or less identical to the raw input. There is no lag between the raw input and the filtered output, so the excessive turn you were seeing by using the odometry yaw velocity data is most likely a result of the raw data.
One more plot:
The blue is the raw absolute yaw measurement, but I took the difference at each timestep with the previous timestep and divided it by the time delta. The red is the raw yaw velocity measurement. Notice that they both start to turn at the same time, but the red stops turning much later.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
| | 15 | No.15 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Keep reading, though, as there are some things you need to look into.Note that IMU is not being used; more on that in a second.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
Now, onto the issues:
First, your IMU timestamps are about 0.03 seconds in the future from all other times. On my machine, this causes the tf message filters I'm using to effectively ignore all of your IMU data. I'm guessing this is happening to you as well, but you can double-check by turning debug on and looking for imuCallback. You'll then see something like this:
WARNING: failed to transform from summit_a/imu->summit_a/base_footprint for imu0 message received at 1418218635.236272155.
I verified that the transforms were, in fact, being broadcast. Double-check what the IMU node is doing, and make sure its timestamps are correct. Does the IMU have an internal clock? How is it setting the timestamp value?
In the rviz output above, do you see how the red value estimate (which was ekf_localization_node's output in the bag file you sent me) keeps going? turning after the initial turn stops for the raw odometry? That's because it's using the raw yaw velocity data. That would normally be fine, except for this:
That's a Matlab plot of your raw odometry measurement yaw velocity after I accumulated it (red), and the absolute measurement (blue). See how the absolute measurement planes out before the velocity? It seems as if the yaw velocity data is lagged behind the absolute yaw data. Again, these are raw measurements, and have not been filtered (at least not on my end; I have no idea what the Summit driver is doing behind the scenes). At first I thought that my filter was trusting its own yaw velocity estimate too much, and was taking too long to fuse the raw measurements, but then I plotted this:
Those values are the raw measurement yaw velocity (from the odometry message) and the filtered values from ekf_localization_node. As you can see, they are directly in sync, meaning that the filter is accepting each measurement with a high degree of confidence, and is moving the filtered yaw velocity output so that it is more or less identical to the raw input. There is no lag between the raw input and the filtered output, so the excessive turn you were seeing by using the odometry yaw velocity data is most likely a result of the raw data.
One more plot:
The blue is the raw absolute yaw measurement, but I took the difference at each timestep with the previous timestep and divided it by the time delta. The red is the raw yaw velocity measurement. Notice that they both start to turn at the same time, but the red stops turning much later.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
| | 16 | No.16 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Note that IMU is not being used; more on that in a second.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
Now, onto the issues:
First, your IMU timestamps are about 0.03 seconds in the future from all other times. On my machine, this causes the tf message filters I'm using to effectively ignore all of your IMU data. I'm guessing this is happening to you as well, but you can double-check by turning debug on and looking for imuCallback. You'll then see something like this:
WARNING: failed to transform from summit_a/imu->summit_a/base_footprint for imu0 message received at 1418218635.236272155.
I verified that the transforms were, in fact, being broadcast. Double-check what the IMU node is doing, and make sure its timestamps are correct. Does the IMU have an internal clock? How is it setting the timestamp value?
In the rviz output above, do you see how the red estimate (which was ekf_localization_node's output in the bag file you sent me) keeps turning after the initial turn stops for the raw odometry? That's because it's using the raw yaw velocity data. That would normally be fine, except for this:
That's a Matlab plot of your raw odometry measurement yaw velocity after I accumulated it (red), and the absolute measurement (blue). See how the absolute measurement planes out before the velocity? It seems as if the yaw velocity data is lagged behind the absolute yaw data. Again, these are raw measurements, and have not been filtered (at least not on my end; I have no idea what the Summit driver is doing behind the scenes). At first I thought that my filter was trusting its own yaw velocity estimate too much, and was taking too long to fuse the raw measurements, but then I plotted this:
Those values are the raw measurement yaw velocity (from the odometry message) and the filtered values from ekf_localization_node. As you can see, they are directly in sync, meaning that the filter is accepting each measurement with a high degree of confidence, and is moving the filtered yaw velocity output so that it is more or less identical to the raw input. There is no lag between the raw input and the filtered output, so the excessive turn you were seeing by using the odometry yaw velocity data is most likely a result of the raw data.
One more plot:
The blue is the raw absolute yaw measurement, but I took the difference at each timestep with the previous timestep and divided it by the time delta. The red is the raw yaw velocity measurement. Notice that they both start to turn at the same time, but the red stops turning much later.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
EDIT 5 (in response to update 4):
OK, I think we may have a disconnect. I can see from your debug text file (and I thank you for providing it) that for you, the IMU data is being accepted. Part of the reason for the tf timing issue I'm having may be that I cannot filter your bag files (I get the error I mentioned previously), so instead I record a new one while I play back the old one, but only record what I want. If we want to sort this out faster, here's what I need (and I plan to put this on the wiki):
ekf_localization_node. Do not record your tf tree, though you should probably provide the values of any static transforms you're broadcasting.Thanks!
| | 17 | No.17 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Note that IMU is not being used; more on that in a second.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
Now, onto the issues:
First, your IMU timestamps are about 0.03 seconds in the future from all other times. On EDIT: Removing these plots, as I've got better ones below, and my machine, this causes the hypothesis as to tf message filters I'm using effectively ignore all of your IMU data. I'm guessing this is happening to you what's wrong with your data has changed as well, but you can double-check by turning debug on and looking for imuCallback. You'll then see something like this:
WARNING: failed to transform from summit_a/imu->summit_a/base_footprint for imu0 message received at 1418218635.236272155.
I verified that the transforms were, in fact, being broadcast. Double-check what the IMU node is doing, and make sure its timestamps are correct. Does the IMU have an internal clock? How is it setting the timestamp value?
In the rviz output above, do you see how the red estimate (which was ekf_localization_node's output in the bag file you sent me) keeps turning after the initial turn stops for the raw odometry? That's because it's using the raw yaw velocity data. That would normally be fine, except for this:
That's a Matlab plot of your raw odometry measurement yaw velocity after I accumulated it (red), and the absolute measurement (blue). well. See how the absolute measurement planes out before the velocity? It seems as if the yaw velocity data is lagged behind the absolute yaw data. Again, these are raw measurements, and have not been filtered (at least not on my end; I have no idea what the Summit driver is doing behind the scenes). At first I thought that my filter was trusting its own yaw velocity estimate too much, and was taking too long to fuse the raw measurements, but then I plotted this:
Those values are the raw measurement yaw velocity (from the odometry message) and the filtered values from ekf_localization_node. As you can see, they are directly in sync, meaning that the filter is accepting each measurement with a high degree of confidence, and is moving the filtered yaw velocity output so that it is more or less identical to the raw input. There is no lag between the raw input and the filtered output, so the excessive turn you were seeing by using the odometry yaw velocity data is most likely a result of the raw data.
One more plot:
The blue is the raw absolute yaw measurement, but I took the difference at each timestep with the previous timestep and divided it by the time delta. The red is the raw yaw velocity measurement. Notice that they both start to turn at the same time, but the red stops turning much later.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
EDIT 5 (in response to update 4):
OK, I think we may have a disconnect. I can see from your debug text file (and I thank you for providing it) that for you, the IMU data is being accepted. Part of the reason for the tf timing issue I'm having may be that I cannot filter your bag files (I get the error I mentioned previously), so instead I record a new one while I play back the old one, but only record what I want. If we want to sort this out faster, here's what I need (and I plan to put this on the wiki):
ekf_localization_node. Do not record your tf tree, though you should probably provide the values of any static transforms you're broadcasting.Thanks!
EDIT 6 (still in response to update 4):
OK, I got some more time to look over your plots and mine, and I believe I've found the issue.
Here's what I did: I played back your Velocities2.bag file that you linked in your question. I then did
rostopic echo /summit_a/summit_xl_controller/odom -p > odom.txt
From the odom.txt file, I imported these columns into Matlab:
Position (X, Y, Z) Orientation (X, Y, Z, W) Angular Velocity (X, Y, Z)
For my first plot, what I want to show is that if you integrate all of your angular velocities over time, you should get more or less the same orientation as your raw orientation data. In other words, we add up all the small changes in yaw that we get at every time step, and it should match the absolute yaw. So I did this in Matlab:
odom_yawvel_integrated = cumsum(odom_yawvel)*0.02; % Your odometry gets reported at 50 Hz
This effectively integrates your yaw velocity from your odometry data. Now I plot both your yaw and yaw velocity data:
figure(1)
plot(odom_yaw);
hold on;
plot(odom_yawvel_integrated, 'r');
legend('Raw odometry yaw', 'Accumulated yaw from raw yaw velocity');
And this is what I get:
What this plot means is that your yaw velocity is too fast. These plots should overlay pretty much on top of one another. Your raw yaw stops around -3 radians after the first turn, but your yaw velocity keeps going until the accumulated value (which is effectively what the filter is doing) is around -4 radians. There's something not jiving between your odometry's orientation data and its yaw velocity data.
Now let's go the other way: we'll plot the raw yaw velocity data from your odometry, and overlay the calculated velocity that we get by differentiating the absolute yaw:
figure(2)
plot(odom_yawvel);
hold on;
plot(diff(odom_yaw)/0.02, 'r'); % Calculating yaw velocity from absolute yaw, using 50 Hz data rate
legend('Raw odometry yaw velocity', 'Calculated yaw velocity from absolute yaw');
This yields:
It's hard to see, so I zoomed in on the data from the first turn:
The blue values are your raw yaw velocities, and you can see that they are faster than your differentiated yaw data.
Most importantly, however, is a plot of your raw odometry yaw velocity and your raw IMU yaw velocity:
figure(3);
plot(odom_wvel_decim); % Still the raw data, but had to decimate so they'd line up in the plot (different frequencies)
hold on;
plot(imu_wvel, 'r');
legend('Raw Odometry Yaw Velocity', 'Raw IMU Yaw Velocity');
This yields:
Again, you can see how the raw odometry yaw velocity is always "faster" than the raw IMU yaw velocity.
Anyway, what I'm trying to show is that one of the issues for you is that you odometry's yaw velocity data is wrong. However, the last launch file I posted should have worked better for you, as I already disabled yaw velocity for your odom0 source and enabled it for your imu0 source. Did that not help? In any case, if you get time, please record a new bag (as instructed above) with my updated settings. Thanks!
| | 18 | No.18 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Note that IMU is not being used; more on that in a second.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
EDIT: Removing these plots, as I've got better ones below, and my hypothesis as to what's wrong with your data has changed as well. See update 6.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
EDIT 5 (in response to update 4):
OK, I think we may have a disconnect. I can see from your debug text file (and I thank you for providing it) that for you, the IMU data is being accepted. Part of the reason for the tf timing issue I'm having may be that I cannot filter your bag files (I get the error I mentioned previously), so instead I record a new one while I play back the old one, but only record what I want. If we want to sort this out faster, here's what I need (and I plan to put this on the wiki):
ekf_localization_node. Do not record your tf tree, though you should probably provide the values of any static transforms you're broadcasting.Thanks!
EDIT 6 (still in response to update 4):
OK, I got some more time to look over your plots and mine, and I believe I've found the issue.
Here's what I did: I played back your Velocities2.bag file that you linked in your question. I then did
rostopic echo /summit_a/summit_xl_controller/odom -p > odom.txt
From the odom.txt file, I imported these columns into Matlab:
Position (X, Y, Z) Orientation (X, Y, Z, W) Angular Velocity (X, Y, Z)
For my first plot, what I want to show is that if you integrate all of your angular velocities over time, you should get more or less the same orientation as your raw orientation data. In other words, we add up all the small changes in yaw that we get at every time step, and it should match the absolute yaw. So I did this in Matlab:
odom_yawvel_integrated = cumsum(odom_yawvel)*0.02; % Your odometry gets reported at 50 Hz
This effectively integrates your yaw velocity from your odometry data. Now I plot both your yaw and yaw velocity data:
figure(1)
plot(odom_yaw);
hold on;
plot(odom_yawvel_integrated, 'r');
legend('Raw odometry yaw', 'Accumulated yaw from raw yaw velocity');
And this is what I get:
What this plot means is that your yaw velocity is too fast. These plots should overlay pretty much on top of one another. Your raw yaw stops around -3 radians after the first turn, but your yaw velocity keeps going until the accumulated value (which is effectively what the filter is doing) is around -4 radians. There's something not jiving between your odometry's orientation data and its yaw velocity data.
Now let's go the other way: we'll plot the raw yaw velocity data from your odometry, and overlay the calculated velocity that we get by differentiating the absolute yaw:
figure(2)
plot(odom_yawvel);
hold on;
plot(diff(odom_yaw)/0.02, 'r'); % Calculating yaw velocity from absolute yaw, using 50 Hz data rate
legend('Raw odometry yaw velocity', 'Calculated yaw velocity from absolute yaw');
This yields:
It's hard to see, so I zoomed in on the data from the first turn:
The blue values are your raw yaw velocities, and you can see that they are faster than your differentiated yaw data.
Most importantly, however, is a plot of your raw odometry yaw velocity and your raw IMU yaw velocity:
figure(3);
plot(odom_wvel_decim); % Still the raw data, but had to decimate so they'd line up in the plot (different frequencies)
hold on;
plot(imu_wvel, 'r');
legend('Raw Odometry Yaw Velocity', 'Raw IMU Yaw Velocity');
This yields:
Again, you can see how the raw odometry yaw velocity is always "faster" than the raw IMU yaw velocity.
Anyway, what I'm trying to show is that one of the issues for you is that you odometry's yaw velocity data is wrong. However, the last launch file I posted should have worked better for you, as I already disabled yaw velocity for your odom0 source and enabled it for your imu0 source. Did that not help? In any case, if you get time, please record a new bag (as instructed above) with my updated settings.
Oh, one more question: can I see your Summit XL launch files? Looking at this:
...it appears as if the odometry data is getting its angular velocity and orientation from an IMU, and not from wheel encoders (unless I'm missing something). Perhaps I'm looking at the wrong file?
Thanks!
| | 19 | No.19 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Note that IMU is not being used; more on that in a second.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
EDIT: Removing these plots, as I've got better ones below, and my hypothesis as to what's wrong with your data has changed as well. See update 6.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
EDIT 5 (in response to update 4):
OK, I think we may have a disconnect. I can see from your debug text file (and I thank you for providing it) that for you, the IMU data is being accepted. Part of the reason for the tf timing issue I'm having may be that I cannot filter your bag files (I get the error I mentioned previously), so instead I record a new one while I play back the old one, but only record what I want. If we want to sort this out faster, here's what I need (and I plan to put this on the wiki):
ekf_localization_node. Do not record your tf tree, though you should probably provide the values of any static transforms you're broadcasting.Thanks!
EDIT 6 (still in response to update 4):
OK, I got some more time to look over your plots and mine, and I believe I've found the issue.
Here's what I did: I played back your Velocities2.bag file that you linked in your question. I then did
rostopic echo /summit_a/summit_xl_controller/odom -p > odom.txt
From the odom.txt file, I imported these columns into Matlab:
Position (X, Y, Z) Orientation (X, Y, Z, W) Angular Velocity (X, Y, Z)
For my first plot, what I want to show is that if you integrate all of your angular velocities over time, you should get more or less the same orientation as your raw orientation data. In other words, we add up all the small changes in yaw that we get at every time step, and it should match the absolute yaw. So I did this in Matlab:
odom_yawvel_integrated = cumsum(odom_yawvel)*0.02; % Your odometry gets reported at 50 Hz
This effectively integrates your yaw velocity from your odometry data. Now I plot both your yaw and yaw velocity data:
figure(1)
plot(odom_yaw);
hold on;
plot(odom_yawvel_integrated, 'r');
legend('Raw odometry yaw', 'Accumulated yaw from raw yaw velocity');
And this is what I get:
What this plot means is that your yaw velocity is too fast. These plots should overlay pretty much on top of one another. Your raw yaw stops around -3 radians after the first turn, but your yaw velocity keeps going until the accumulated value (which is effectively what the filter is doing) is around -4 radians. There's something not jiving between your odometry's orientation data and its yaw velocity data.
Now let's go the other way: we'll plot the raw yaw velocity data from your odometry, and overlay the calculated velocity that we get by differentiating the absolute yaw:
figure(2)
plot(odom_yawvel);
hold on;
plot(diff(odom_yaw)/0.02, 'r'); % Calculating yaw velocity from absolute yaw, using 50 Hz data rate
legend('Raw odometry yaw velocity', 'Calculated yaw velocity from absolute yaw');
This yields:
It's hard to see, so I zoomed in on the data from the first turn:
The blue values are your raw yaw velocities, and you can see that they are faster than your differentiated yaw data.
Most importantly, however, is a plot of your raw odometry yaw velocity and your raw IMU yaw velocity:
figure(3);
plot(odom_wvel_decim); plot(odom_yawvel_decim); % Still the raw data, but had to decimate so they'd line up in the plot (different frequencies)
hold on;
plot(imu_wvel, 'r');
legend('Raw Odometry Yaw Velocity', 'Raw IMU Yaw Velocity');
This yields:
Again, you can see how the raw odometry yaw velocity is always "faster" than the raw IMU yaw velocity.
Anyway, what I'm trying to show is that one of the issues for you is that you odometry's yaw velocity data is wrong. However, the last launch file I posted should have worked better for you, as I already disabled yaw velocity for your odom0 source and enabled it for your imu0 source. Did that not help? In any case, if you get time, please record a new bag (as instructed above) with my updated settings.
Oh, one more question: can I see your Summit XL launch files? Looking at this:
...it appears as if the odometry data is getting its angular velocity and orientation from an IMU, and not from wheel encoders (unless I'm missing something). Perhaps I'm looking at the wrong file?
Thanks!
| | 20 | No.20 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Note that IMU is not being used; more on that in a second.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
EDIT: Removing these plots, as I've got better ones below, and my hypothesis as to what's wrong with your data has changed as well. See update 6.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
EDIT 5 (in response to update 4):
OK, I think we may have a disconnect. I can see from your debug text file (and I thank you for providing it) that for you, the IMU data is being accepted. Part of the reason for the tf timing issue I'm having may be that I cannot filter your bag files (I get the error I mentioned previously), so instead I record a new one while I play back the old one, but only record what I want. If we want to sort this out faster, here's what I need (and I plan to put this on the wiki):
ekf_localization_node. Do not record your tf tree, though you should probably provide the values of any static transforms you're broadcasting.Thanks!
EDIT 6 (still in response to update 4):
OK, I got some more time to look over your plots and mine, and I believe I've found the issue.
Here's what I did: I played back your Velocities2.bag file that you linked in your question. I then did
rostopic echo /summit_a/summit_xl_controller/odom -p > odom.txt
From the odom.txt file, I imported these columns into Matlab:
Position (X, Y, Z) Orientation (X, Y, Z, W) Angular Velocity (X, Y, Z)
For my first plot, what I want to show is that if you integrate all of your angular velocities over time, you should get more or less the same orientation as your raw orientation data. In other words, we add up all the small changes in yaw that we get at every time step, and it should match the absolute yaw. So I did this in Matlab:
odom_yawvel_integrated = cumsum(odom_yawvel)*0.02; % Your odometry gets reported at 50 Hz
This effectively integrates your yaw velocity from your odometry data. Now I plot both your yaw and yaw velocity data:
figure(1)
plot(odom_yaw);
hold on;
plot(odom_yawvel_integrated, 'r');
legend('Raw odometry yaw', 'Accumulated yaw from raw yaw velocity');
And this is what I get:
What this plot means is that your yaw velocity is too fast. These plots should overlay pretty much on top of one another. Your raw yaw stops around -3 radians after the first turn, but your yaw velocity keeps going until the accumulated value (which is effectively what the filter is doing) is around -4 radians. There's something not jiving between your odometry's orientation data and its yaw velocity data.
Now let's go the other way: we'll plot the raw yaw velocity data from your odometry, and overlay the calculated velocity that we get by differentiating the absolute yaw:
figure(2)
plot(odom_yawvel);
hold on;
plot(diff(odom_yaw)/0.02, 'r'); % Calculating yaw velocity from absolute yaw, using 50 Hz data rate
legend('Raw odometry yaw velocity', 'Calculated yaw velocity from absolute yaw');
This yields:
It's hard to see, so I zoomed in on the data from the first turn:
The blue values are your raw yaw velocities, and you can see that they are faster than your differentiated yaw data.
Most importantly, however, is a plot of your raw odometry yaw velocity and your raw IMU yaw velocity:
figure(3);
plot(odom_yawvel_decim); % Still the raw data, but had to decimate so they'd line up in the plot (different frequencies)
hold on;
plot(imu_wvel, plot(imu_yawvel, 'r');
legend('Raw Odometry Yaw Velocity', 'Raw IMU Yaw Velocity');
This yields:
Again, you can see how the raw odometry yaw velocity is always "faster" than the raw IMU yaw velocity.
Anyway, what I'm trying to show is that one of the issues for you is that you odometry's yaw velocity data is wrong. However, the last launch file I posted should have worked better for you, as I already disabled yaw velocity for your odom0 source and enabled it for your imu0 source. Did that not help? In any case, if you get time, please record a new bag (as instructed above) with my updated settings.
Oh, one more question: can I see your Summit XL launch files? Looking at this:
...it appears as if the odometry data is getting its angular velocity and orientation from an IMU, and not from wheel encoders (unless I'm missing something). Perhaps I'm looking at the wrong file?
Thanks!
| | 21 | No.21 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Note that IMU is not being used; more on that in a second.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
EDIT: Removing these plots, as I've got better ones below, and my hypothesis as to what's wrong with your data has changed as well. See update 6.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
EDIT 5 (in response to update 4):
OK, I think we may have a disconnect. I can see from your debug text file (and I thank you for providing it) that for you, the IMU data is being accepted. Part of the reason for the tf timing issue I'm having may be that I cannot filter your bag files (I get the error I mentioned previously), so instead I record a new one while I play back the old one, but only record what I want. If we want to sort this out faster, here's what I need (and I plan to put this on the wiki):
ekf_localization_node. Do not record your tf tree, though you should probably provide the values of any static transforms you're broadcasting.Thanks!
EDIT 6 (still in response to update 4):
OK, I got some more time to look over your plots and mine, and I believe I've found the issue.
Here's what I did: I played back your Velocities2.bag file that you linked in your question. I then did
rostopic echo /summit_a/summit_xl_controller/odom -p > odom.txt
From the odom.txt file, I imported these columns into Matlab:
Position (X, Y, Z) Orientation (X, Y, Z, W) Angular Velocity (X, Y, Z)
For my first plot, what I want to show is that if you integrate all of your angular velocities over time, you should get more or less the same orientation as your raw orientation data. In other words, we add up all the small changes in yaw that we get at every time step, and it should match the absolute yaw. So I did this in Matlab:
odom_yawvel_integrated = cumsum(odom_yawvel)*0.02; % Your odometry gets reported at 50 Hz
This effectively integrates your yaw velocity from your odometry data. Now I plot both your yaw and yaw velocity data:
figure(1)
plot(odom_yaw);
hold on;
plot(odom_yawvel_integrated, 'r');
legend('Raw odometry yaw', 'Accumulated yaw from raw yaw velocity');
And this is what I get:
What this plot means is that your yaw velocity is too fast. These plots should overlay pretty much on top of one another. Your raw yaw stops around -3 radians after the first turn, but your yaw velocity keeps going until the accumulated value (which is effectively what the filter is doing) is around -4 radians. There's something not jiving between your odometry's orientation data and its yaw velocity data.
Now let's go the other way: we'll plot the raw yaw velocity data from your odometry, and overlay the calculated velocity that we get by differentiating the absolute yaw:
figure(2)
plot(odom_yawvel);
hold on;
plot(diff(odom_yaw)/0.02, 'r'); % Calculating yaw velocity from absolute yaw, using 50 Hz data rate
legend('Raw odometry yaw velocity', 'Calculated yaw velocity from absolute yaw');
This yields:
It's hard to see, so I zoomed in on the data from the first turn:
The blue values are your raw yaw velocities, and you can see that they are faster than your differentiated yaw data.
Most importantly, however, is a plot of your raw odometry yaw velocity and your raw IMU yaw velocity:
figure(3);
plot(odom_yawvel_decim); % Still the raw data, but had to decimate so they'd line up in the plot (different frequencies)
hold on;
plot(imu_yawvel, 'r');
legend('Raw Odometry Yaw Velocity', 'Raw IMU Yaw Velocity');
This yields:
Again, you can see how the raw odometry yaw velocity is always "faster" than the raw IMU yaw velocity.
Anyway, what I'm trying to show is that one of the issues for you is that you odometry's yaw velocity data is wrong. However, the last launch file I posted should have worked better for you, as I already disabled yaw velocity for your odom0 source and enabled it for your imu0 source. Did that not help? In any case, if you get time, please record a new bag (as instructed above) with my updated settings.
Oh, one more question: can I see your Summit XL launch files? Looking at this:
...it appears as if the odometry data is getting its angular velocity and orientation from an IMU, and not from wheel encoders (unless I'm missing something). Perhaps I'm looking at the wrong file?
Thanks!
I just plotted your raw odometry and the "new" filtered output (from your last update) on top of one another. Here's what I see:
The two end up ~2 cm apart from one another. As it would be hard to measure that difference in the real world, how are you making the determination that the filter isn't any "better" than the raw odometry? I guess what I want to know is what you were expecting to see from the data. Also, I'm interested to know where your Summit XL is getting its orientation and angular velocity data for its odometry message.
| | 22 | No.22 Revision |
Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?
To your questions:
world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="imu0_differential" value="true"/>
Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?
The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.
Side note 1: gravitational acceleration only applies to IMU topics. You can remove
<param name="odom0_remove_gravitational_acceleration" value="true"/>
Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:
<rosparam param="odom0_config">[false, false, true,
true, true, true,
true, true, true,
true, true, true,
false, false, false]</rosparam>
Fix those issues and post your messages and I can help more.
EDIT after update:
Two things:
Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).
For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.
Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.
EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:
<param name="imu0" value="topic_of_your_imu"/>
EDIT 3 (in response to update 2):
You have a few culprits:
First, your covariances. See these pages:
http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data
Here is your IMU's angular velocity:
angular_velocity:
x: 0.00209439505558
y: -0.00191986216679
z: 1.14231794217
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.
Here is your odometry pose data:
pose:
position:
x: -0.18140229997
y: 1.41788893847
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.956499625695
w: 0.291733553171
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
...and here is your odometry twist data:
twist:
linear:
x: 0.442822994916
y: -0.298867051238
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.83410439336
covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.
There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.
Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?
So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).
EDIT 4 (in response to update 3):
Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.
<launch>
<node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
<node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
<node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
<node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />
<node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>
<group ns="summit_a">
<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="odom_frame" value="summit_a/odom"/>
<param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
<param name="imu0" value="vbox3i/Imu"/>
<rosparam param="odom0_config">[false, false, false,
false, false, true,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug.txt"/>
</node>
</group>
</launch>
Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Note that IMU is not being used; more on that in a second.
Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.
EDIT: Removing these plots, as I've got better ones below, and my hypothesis as to what's wrong with your data has changed as well. See update 6.
For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.
EDIT 5 (in response to update 4):
OK, I think we may have a disconnect. I can see from your debug text file (and I thank you for providing it) that for you, the IMU data is being accepted. Part of the reason for the tf timing issue I'm having may be that I cannot filter your bag files (I get the error I mentioned previously), so instead I record a new one while I play back the old one, but only record what I want. If we want to sort this out faster, here's what I need (and I plan to put this on the wiki):
ekf_localization_node. Do not record your tf tree, though you should probably provide the values of any static transforms you're broadcasting.Thanks!
EDIT 6 (still in response to update 4):
OK, I got some more time to look over your plots and mine, and I believe I've found the issue.
Here's what I did: I played back your Velocities2.bag file that you linked in your question. I then did
rostopic echo /summit_a/summit_xl_controller/odom -p > odom.txt
From the odom.txt file, I imported these columns into Matlab:
Position (X, Y, Z) Orientation (X, Y, Z, W) Angular Velocity (X, Y, Z)
For my first plot, what I want to show is that if you integrate all of your angular velocities over time, you should get more or less the same orientation as your raw orientation data. In other words, we add up all the small changes in yaw that we get at every time step, and it should match the absolute yaw. So I did this in Matlab:
odom_yawvel_integrated = cumsum(odom_yawvel)*0.02; % Your odometry gets reported at 50 Hz
This effectively integrates your yaw velocity from your odometry data. Now I plot both your yaw and yaw velocity data:
figure(1)
plot(odom_yaw);
hold on;
plot(odom_yawvel_integrated, 'r');
legend('Raw odometry yaw', 'Accumulated yaw from raw yaw velocity');
And this is what I get:
What this plot means is that your yaw velocity is too fast. These plots should overlay pretty much on top of one another. Your raw yaw stops around -3 radians after the first turn, but your yaw velocity keeps going until the accumulated value (which is effectively what the filter is doing) is around -4 radians. There's something not jiving between your odometry's orientation data and its yaw velocity data.
Now let's go the other way: we'll plot the raw yaw velocity data from your odometry, and overlay the calculated velocity that we get by differentiating the absolute yaw:
figure(2)
plot(odom_yawvel);
hold on;
plot(diff(odom_yaw)/0.02, 'r'); % Calculating yaw velocity from absolute yaw, using 50 Hz data rate
legend('Raw odometry yaw velocity', 'Calculated yaw velocity from absolute yaw');
This yields:
It's hard to see, so I zoomed in on the data from the first turn:
The blue values are your raw yaw velocities, and you can see that they are faster than your differentiated yaw data.
Most importantly, important, however, is a plot of your raw odometry yaw velocity and your raw IMU yaw velocity:
figure(3);
plot(odom_yawvel_decim); % Still the raw data, but had to decimate so they'd line up in the plot (different frequencies)
hold on;
plot(imu_yawvel, 'r');
legend('Raw Odometry Yaw Velocity', 'Raw IMU Yaw Velocity');
This yields:
Again, you can see how the raw odometry yaw velocity is always "faster" than the raw IMU yaw velocity.
Anyway, what I'm trying to show is that one of the issues for you is that you odometry's yaw velocity data is wrong. However, the last launch file I posted should have worked better for you, as I already disabled yaw velocity for your odom0 source and enabled it for your imu0 source. Did that not help? In any case, if you get time, please record a new bag (as instructed above) with my updated settings.
Oh, one more question: can I see your Summit XL launch files? Looking at this:
...it appears as if the odometry data is getting its angular velocity and orientation from an IMU, and not from wheel encoders (unless I'm missing something). Perhaps I'm looking at the wrong file?
EDIT 7 (yes, still in response to update 4):
I just plotted your raw odometry and the "new" filtered output (from your last update) on top of one another. Here's what I see:
The two end up ~2 cm apart from one another. As it would be hard to measure that difference in the real world, how are you making the determination that the filter isn't any "better" than the raw odometry? I guess what I want to know is what you were expecting to see from the data. Also, I'm interested to know where your Summit XL is getting its orientation and angular velocity data for its odometry message.
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.