ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# Turtlebot pose wrong when other velocity input

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.

edit retag close merge delete

What are you using for odometry measurements?

( 2012-06-25 03:09:38 -0500 )edit

In my obstacle avoidance, I don't use any odometry. I just set an the appropriate angular speed to avoid, and when no obstacle is detected afterwards, the navigation (that was still running when avoiding) should have found from the automatically updated localization a new path to reach the goal.

( 2012-06-25 05:10:10 -0500 )edit

Sort by » oldest newest most voted

Actually, this problem of wrong pose of the robot was coming from a bad gyroscope. I managed to narrow down the problem there, observing the imu data. I deactivated the gyro to prevent this problem (has_gyro = false), but the localization becomes less precise. I guess I will have to change this gyro.

more

If you're not publishing an odometry source to the navigation stack, it will have no idea that the robot has moved. Odometry is the measure of how far the robot has traveled since the odometry frame was set. The navigation stack depends on this information to know where the robot is with respect to its environment. If you command the robot to move, but the robot doesn't know that it has moved, it will not adjust its trajectories because it believes that it's still in the same position that it started in.

more

I don't need to publish odometry to the navigation stack, it already does it on its own. The navigation is still running in background when I avoid the obstacle, I just choose to discard the velocity command from it, but the robot should still know where it is.

( 2012-06-25 19:56:34 -0500 )edit