Turtlebot pose wrong when other velocity input

edit

I have built a node with the purpose of avoiding obstacles with the 3D optical flow via the Kinect on a Turtlebot. To include this behaviour in the navigation, I first removed the automatic obstacle avoidance (obstacle_range parameter set to 1cm in costmap_common_params.yaml inside the turtlebot_navigation package). Then I remapped the output velocity command of move_base to my node velocity subscription, and remapped the input velocity command of turtlebot_node to my node velocity publication. This is done in order to select in my node, which velocity I will publish: the one coming from the navigation or the one coming from the obstacle avoidance (angular.z speed). This works, and gives the appropriate obstacle avoidance behaviour, but the robot does not come back to its path to reach the goal. The navigation is still running during the obstacle avoidance (even though the velocities published by move_base are not used), but it does not seem to update correctly its pose in Rviz and generate a new trajectory. In Rviz, the robot still believes to be on the right path and go straight forward, whereas in reality the robot has turned. At some point, it gets lost. Any clue where the odometry update does not do its job and the trajectory planner does not generate a new path ? I need the robot to reach the goal after avoiding the obstacle, so to re-join the path.

I precise that the navigation continues to run while I avoid the obstacle, and the path is updated automatically via the odometry by the trajectory_planner_ros node (lines 396-411):

// Set current velocities from odometry

geometry_msgs::Twist global_vel;

odom_lock_.lock();
global_vel.linear.x = base_odom_.twist.twist.linear.x;
global_vel.linear.y = base_odom_.twist.twist.linear.y;
global_vel.angular.z = base_odom_.twist.twist.angular.z;
odom_lock_.unlock();

tf::Stamped<tf::Pose> drive_cmds;
drive_cmds.frame_id_ = robot_base_frame_;

tf::Stamped<tf::Pose> robot_vel;
robot_vel.setData(btTransform(tf::createQuaternionFromYaw(global_vel.angular.z), btVector3(global_vel.linear.x, global_vel.linear.y, 0)));
robot_vel.frame_id_ = robot_base_frame_;
robot_vel.stamp_ = ros::Time();

If it can help, I looked up for robot_pose_ekf/odom during the navigation process. When there is no obstacle, the pose displayed is correct, when there is an obstacle (My node feeding other velocities to turtlebot_node than the one from the navigation), the robot pose becomes wrong. Any idea why introducing other velocities can mess up the odometry ? I precise that /odom values are good, only robot_pose_ekf/odom is wrong.