Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Navigation of turtlebot (Moving 1m forward, 1m left)

I want to move my turtlebot 2 a fixed set of distances such as 1m forward, 1m right and etc accurately using odometry. This is the C++ code i have currently. However, it does not seem to be using the odometry. How should I change the code to enable myself to move the robot accurately ?

Below is the code that I currently have. What it does is enable me to move the turtlebot as well as received odometry data on the console.

    class TurtleMoveAvoid;

    class TurtleMoveAvoid {
    public:
    TurtleMoveAvoid() {
  _pub = _n.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity", 1);
  _sub = _n.subscribe("/odom", 1, &TurtleMoveAvoid::odomCallback, this);
  _left = true;
  _right = true;
}

void publish() {
  if (_left && _right) {
    forward();
  } else if (_right) {
    turnRight();
  } else if (_left) {
    turnLeft();
  } else {
    reverse();
  }
}

void forward() {
  geometry_msgs::Twist vel;
  vel.linear.x = 0.2;
  vel.angular.z = 0.0;
  _pub.publish(vel);
}
void reverse() {
  geometry_msgs::Twist vel;
  vel.linear.x = -0.2;
  vel.angular.z = 0.0;
  _pub.publish(vel);
}
void turnLeft() {
  geometry_msgs::Twist vel;
  vel.linear.x = 0.0;
  vel.angular.z = 0.7;
  _pub.publish(vel);
}

void turnRight() {
  geometry_msgs::Twist vel;
  vel.linear.x = 0.0;
  vel.angular.z = -0.7;
  _pub.publish(vel);
}

void odomCallback(const nav_msgs::Odometry::ConstPtr& odom) {
ROS_INFO("I received odom: [%f,%f,%f]", odom->twist.twist.linear.x, odom->pose.pose.position.y,odom->pose.pose.orientation.z);
double xpos = odom->twist.twist.linear.x;
if(xpos < 0.2) //assuming 0.2 is the end
{
    _right = true;
    _left = true;
}
else
{
    _right = false;
    _left = false;
}

publish();
}

 private:
 ros::NodeHandle _n; 
 ros::Publisher _pub;
 ros::Subscriber _sub;
 bool _right, _left;
 };// End of class TurtleMoveAvoid

 int main(int argc, char **argv) {
 ros::init(argc, argv, "turtle_move_odom");
 TurtleMoveAvoid turtle;
 ros::Rate loop_rate(10);

 while (ros::ok())
 {
ros::spinOnce(); //get new callback, adjust publish then publish again.
    loop_rate.sleep();
 }
 return 0;
 }