Hi Mathieu,
Following
this approach, I implemented a node to reset visual odometry when the tracking is lost (null transform received). I am using the last XY pose from wheel odometry and the yaw heading from an IMU sensor, this is the code calling the "reset_odom_to_pose" service after gathering odometries in a approximated time callback:
void fuseOdometry(const Odometry::ConstPtr &wheel_odom, const Odometry::ConstPtr &visual_odom, const Imu::ConstPtr &imu_data,
ros::Publisher* publisher, ros::ServiceClient* client)
{
Odometry fused_odom = *visual_odom;
Point visual_pose = visual_odom->pose.pose.position;
Quaternion visual_quat = visual_odom->pose.pose.orientation;
if (visual_pose.x == 0 && visual_pose.y == 0 && visual_pose.z == 0 &&
visual_quat.x == 0 && visual_quat.y == 0 && visual_quat.z == 0 && visual_quat.w == 0)
{
ROS_DEBUG("Null visual odometry detected! Fusing wheels and IMU measurements...");
fused_odom.pose.pose.position.x = wheel_odom->pose.pose.position.x;
fused_odom.pose.pose.position.y = wheel_odom->pose.pose.position.y;
fused_odom.pose.pose.orientation = imu_data->orientation;
rtabmap_ros::ResetPose::Request request;
request.x = wheel_odom->pose.pose.position.x;
request.y = wheel_odom->pose.pose.position.y;
request.z = 0;
request.roll = 0;
request.pitch = 0;
request.yaw = tf::getYaw(imu_data->orientation);
ROS_INFO("YAW = %f", request.yaw);
rtabmap_ros::ResetPose::Response response;
if (client->call(request, response))
ROS_INFO("Visual odometry successfully reset");
else
ROS_ERROR("Unable to reset visual odometry!");
}
publisher->publish(fused_odom);
}
When null odometry is detected, the service is correctly called and response received, but even if I set only XYH values (I have a non-holonomic planar robot), rgbd_odometry gives this error:
[ INFO] [1449495190.331007237]: visual_odoemtry: reset odom to pose xyz=0.369134,-0.075018,0.000000 rpy=0.000000,0.000000,1.463692
[ WARN] (2015-12-07 14:33:10.327) Odometry.cpp:129::reset() Force2D=true and the initial pose contains
z, roll or pitch values (xyz=0.369134,-0.075018,0.000000 rpy=0.000000,0.000000,1.463692). They are set to null.
[ INFO] [1449495191.331007237]: Visual odometry successfully reset
Using RViz to display this, I see that visual odometry seems to align with wheel odometry, but the yaw is always zero and many times the position is wrong, too... am I missing something?
Thanks,
Guido
~Guido
Locked
