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;
}