odom velocity mesaurement are not relative to vehicle frame
CaptKrasno opened this issue · 9 comments
Describe the bug
The velocity measurement in nav/relative_pos/odom are relative to vehicle frame even with:
filter_vel_in_vehicle_frame: true # default value
In fact, the velocities don't seem to consistently match those int the ENU/NED frame
See the screenshots below
In the above example the vehicle is moving straight ahead at 1.5 m/s. I would expect to see nav/relative_pos/odom/twist/twist/x ~= 1.5 and nav/relative_pos/odom/twist/twist/y ~= 0 in this case. However, as you can see In this screenshot the nav/relative_pos/odom velocities seem reasonable for the ENU frame given the plots of x/y position.
The above is an earlier part of the same recording. In this screenshot you can see that even though the x position has a negative slope nav/relative_pos/odom/twist/twist/x ~= 0. The y position has a slope near zero slope but nav/relative_pos/odom/twist/twist/y ~= -1.5
To Reproduce
Steps to reproduce the behavior:
- use this launch file with default arguments asv-trevor-launch
- see the associated yaml file
Expected behavior
See screenshots above
Environment (please complete the following information):
- OS: Ubuntu 20.04
- Architecture: arm64 and x86
- ROS Version: foxy
- Version: 3.0.1
- Sensor(s): 3DM-GQ7
Modifications
None
Launch Parameters
asv-trevor-launch
[associated yaml file](https://github.com/USF-COMIT/trevor/blob/f9036dc1f14a78f6f4afaaae9b82e18573287a4c/trevor/config/microstrain.yaml
I'm seeing the same issue.
Specifically, I have set filter_vel_in_vehicle_frame to True in my config.
I have a GQ7 rigidly attached to an Ackermann steered vehicle, which I'm driving relatively slowly in a parking lot in a half circle. In the base_link frame I would expect the linear velocity to be almost entirely in the x direction. I don't see that.
If I integrate the velocities in the x direction and the separately integrate the y velocities the resulting path looks like the half circle I actually drove, implying the velocities are being reported in a fixed frame, not base_link.
I modified the driver to add the following log statement:
diff --git a/src/publishers.cpp b/src/publishers.cpp
index 983e8f7..7fd686b 100644
--- a/src/publishers.cpp
+++ b/src/publishers.cpp
@@ -736,6 +736,7 @@ void Publishers::handleFilterAttitudeQuaternion(const mip::data_filter::Attitude
// Save the quaternion for later
filter_attitude_quaternion_ = tf2::Quaternion(attitude_quaternion.q[1], attitude_quaternion.q[2], attitude_quaternion.q[3], attitude_quaternion.q[0]);
+ ROS_ERROR_STREAM("Saving for later. " << filter_attitude_quaternion_.getAngle());
}
void Publishers::handleFilterEulerAnglesUncertainty(const mip::data_filter::EulerAnglesUncertainty& euler_angles_uncertainty, const uint8_t descriptor_set, mip::Timestamp timestamp)
@@ -781,6 +782,7 @@ void Publishers::handleFilterVelocityNed(const mip::data_filter::VelocityNed& ve
// If we are publishing velocity in the vehicle frame, rotate the velocity using the attitude
if (config_->filter_vel_in_vehicle_frame_)
{
+ ROS_ERROR_STREAM("Rotating filter to vehicle frame. Using: " << filter_attitude_quaternion_.getAngle());
const tf2::Vector3 filter_current_vel(filter_odom_msg->twist.twist.linear.x, filter_odom_msg->twist.twist.linear.y, filter_odom_msg->twist.twist.linear.z);
const tf2::Vector3 filter_rotated_vel = tf2::quatRotate(filter_attitude_quaternion_.inverse(), filter_current_vel);
filter_odom_msg->twist.twist.linear.x = filter_rotated_vel.getX();
and can tell from that that when I'm running the driver, the driver does think config->filter_vel_in_vehicle_frame_ is true and that the angle in fitler_atittude_quaternion_ is changing logically.
I need to get setup to test, but we were using the NED rotation to rotate the velocity even when use_enu_frame was true. I will do some testing ASAP to see if this fixes your issue, but in the meantime if anyone wants to test, the code is in the following branches
Hi!
I tried running bugfix/ros_vel_in_sensor_frame on our vehicle and it appears to have fixed the issue, though as you note in the change to the default params, it puts the velocity in the sensor frame, not the vehicle frame, which is fine by me, I can handle that in tf by properly labeling the frames in the config.
Thanks for the quick turnaround!
This issue is stale because it has been open for 2 weeks with no activity. If the issue is still not resolved, please leave a comment describing what is still not working
Hi @robbiefish, we've been running with the fix in https://github.com/LORD-MicroStrain/microstrain_inertial/tree/bugfix/ros_vel_in_sensor_frame for the last few weeks, and it appears to fix the issue described in this Issue.
Are there any plans to have that branch merged and released on the build farm?
This issue is stale because it has been open for 2 weeks with no activity. If the issue is still not resolved, please leave a comment describing what is still not working
This issue was closed because it has been inactive for 2 weeks since being marked as stale. If the issue is still not resolved, please reopen the issue, and leave a comment describing what is still not working
I tested the bugfix/ros2_vel_in_sensor_frame branch with the 3DM-GQ7 device and it is not working correctly.
This is my setup:
- Device: 3DM-GQ7
- Firmware: 1.1.02
- Distribution: ROS2 Humble
- Mounting: x-axis pointing forward, y-axis pointing to the right, and z-axis pointing downward
I drove a short round in clockwise direction for each of these 4 parameter configurations:
- ENU ENU:
use_enu_frame:=trueandfilter_vel_in_vehicle_frame:=false - ENU VEH:
use_enu_frame:=trueandfilter_vel_in_vehicle_frame:=true - NED NED:
use_enu_frame:=falseandfilter_vel_in_vehicle_frame:=false - NED VEH:
use_enu_frame:=falseandfilter_vel_in_vehicle_frame:=true
As can be seen in the following plot, the x-acceleration (topic imu/data; field linear_acceleration.x) is positive during the first 6 seconds for all configurations. But the x-velocity (topic nav/odom; field twist.twist.linear.x) goes negative even though the x-axis points forward and filter_sensor2vehicle_frame_transformation_euler:=[0.0, 0.0, 0.0].
I have attached the 4 parameter files used during the measurements:
parameter_files.zip