free_space_navigation.cpp
Go to the documentation of this file.
00001 /*
00002  *  Gaitech Educational Portal
00003  *
00004  * Copyright (c) 2016
00005  * All rights reserved.
00006  *
00007  * License Type: GNU GPL
00008  *
00009  *   This program is free software: you can redistribute it and/or modify
00010  *   it under the terms of the GNU General Public License as published by
00011  *   the Free Software Foundation, either version 3 of the License, or
00012  *   (at your option) any later version.
00013  *   This program is distributed in the hope that it will be useful,
00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *   GNU General Public License for more details.
00017  *
00018  *   You should have received a copy of the GNU General Public License
00019  *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
00020  *
00021  *    Program: Free-Space Navigation
00022  *
00023  */
00024 
00025 #include "ros/ros.h"
00026 #include "geometry_msgs/Twist.h"
00027 #include "sensor_msgs/LaserScan.h"
00028 #include "nav_msgs/Odometry.h"
00029 #include "tf/tf.h"
00030 #include <tf/transform_listener.h>
00031 #include <fstream>
00032 
00033 using namespace std;
00034 #define LINEAR_VELOCITY_MINIMUM_THRESHOLD 0.2
00035 #define ANGULAR_VELOCITY_MINIMUM_THRESHOLD 0.4
00036 
00037 //declare publishers
00038 ros::Publisher velocityPublisher;
00039 //declare subscribers
00040 ros::Subscriber scanSubscriber;
00041 ros::Subscriber pose_subscriber;
00042 //global variable to update the position of the robot
00043 nav_msgs::Odometry turtlebot_odom_pose;
00044 
00045 
00046 
00047 
00048 //callback function for the /odom topic to update the pose
00049 void poseCallback(const nav_msgs::Odometry::ConstPtr & pose_message);
00050 //the function that makes the robot moves forward and backward
00051 void move_v1(double speed, double distance, bool isForward);
00052 void move_v2(double speed, double distance, bool isForward);
00053 void move_v3(double speed, double distance, bool isForward);
00054 //the function that makes the robot rotates left and right
00055 double rotate(double ang_vel, double angle_radian, bool isClockwise);
00056 double degree2radian(double degreeAngle);
00057 double radian2degree(double radianAngle);
00058 double calculateYaw( double x1, double y1, double x2,double y2);
00059 
00060 void moveSquare(double sideLength);
00061 
00062 int main(int argc, char **argv){
00063 
00064         //initialize the ROS node
00065         ros::init(argc, argv, "free_space_navigation_node");
00066         ros::NodeHandle n;
00067 
00068         //subscribe to the odometry topic to get the position of the robot
00069         pose_subscriber = n.subscribe("/odom", 10, poseCallback);
00070         //register the velocity publisher
00071         velocityPublisher =n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/teleop", 1000);
00072 
00073         ros::spinOnce();
00074         ros::Rate loop(1);
00075         loop.sleep();loop.sleep();loop.sleep();//loop.sleep();loop.sleep();
00076         ros::spinOnce();
00077 
00078         while (ros::ok()){
00079                 ros::spinOnce();loop.sleep();
00080                 printf("robot initial pose: (%.2f, %.2f, %.2f)\n",
00081                                                                                 turtlebot_odom_pose.pose.pose.position.x,
00082                                                                                 turtlebot_odom_pose.pose.pose.position.y,
00083                                                                                 radian2degree(tf::getYaw(turtlebot_odom_pose.pose.pose.orientation)));
00084                 moveSquare(1.0);
00085                 //exercise: try to remove the ros::SpinOnce() and observe and comment the result
00086                 ros::spinOnce();loop.sleep();ros::spinOnce();
00087                 printf("robot final pose: (%.2f, %.2f, %.2f)\n",
00088                                                                                         turtlebot_odom_pose.pose.pose.position.x,
00089                                                                                         turtlebot_odom_pose.pose.pose.position.y,
00090                                                                                         radian2degree(tf::getYaw(turtlebot_odom_pose.pose.pose.orientation)));
00091                 return 0;
00092         }
00093 
00094 
00095 
00096         return 0;
00097 }
00098 
00099 
00100 
00101 
00102 void poseCallback(const nav_msgs::Odometry::ConstPtr & pose_message){
00103         turtlebot_odom_pose.pose.pose.position.x=pose_message->pose.pose.position.x;
00104         turtlebot_odom_pose.pose.pose.position.y=pose_message->pose.pose.position.y;
00105         turtlebot_odom_pose.pose.pose.position.z=pose_message->pose.pose.position.z;
00106 
00107         turtlebot_odom_pose.pose.pose.orientation.w=pose_message->pose.pose.orientation.w;
00108         turtlebot_odom_pose.pose.pose.orientation.x=pose_message->pose.pose.orientation.x;
00109         turtlebot_odom_pose.pose.pose.orientation.y=pose_message->pose.pose.orientation.y;
00110         turtlebot_odom_pose.pose.pose.orientation.z=pose_message->pose.pose.orientation.z;
00111 }
00112 
00113 void moveSquare(double sideLength){
00114         for (int i=0;i<4;i++){
00115                 move_v1(0.3, sideLength, true);
00116                 rotate (0.3, degree2radian(90), true);
00117         }
00118 }
00119 
00120 
00129 void move_v1(double speed, double distance, bool isForward){
00130         //declare a Twist message to send velocity commands
00131         geometry_msgs::Twist VelocityMessage;
00132         //declare tf transform listener: this transform listener will be used to listen and capture the transformation between
00133         // the /odom frame (that represent the reference frame) and the base_footprint frame the represent moving frame
00134         tf::TransformListener listener;
00135         //declare tf transform
00136         //init_transform: is the transformation before starting the motion
00137         tf::StampedTransform init_transform;
00138         //current_transformation: is the transformation while the robot is moving
00139         tf::StampedTransform current_transform;
00140 
00141 
00142         //set the linear velocity to a positive value if isFoward is true
00143         if (isForward)
00144                 VelocityMessage.linear.x =abs(speed);
00145         else //else set the velocity to negative value to move backward
00146                 VelocityMessage.linear.x =-abs(speed);
00147         //all velocities of other axes must be zero.
00148         VelocityMessage.linear.y =0;
00149         VelocityMessage.linear.z =0;
00150         //The angular velocity of all axes must be zero because we want  a straight motion
00151         VelocityMessage.angular.x = 0;
00152         VelocityMessage.angular.y = 0;
00153         VelocityMessage.angular.z =0;
00154 
00155         double distance_moved = 0.0;
00156         ros::Rate loop_rate(10); // we publish the velocity at 10 Hz (10 times a second)
00157 
00158         /*
00159          * First, we capture the initial transformation before starting the motion.
00160          * we call this transformation "init_transform"
00161          * It is important to "waitForTransform" otherwise, it might not be captured.
00162          */
00163         try{
00164                 //wait for the transform to be found
00165                 listener.waitForTransform("/base_footprint", "/odom", ros::Time(0), ros::Duration(10.0) );
00166                 //Once the transform is found,get the initial_transform transformation.
00167                 listener.lookupTransform("/base_footprint", "/odom",ros::Time(0), init_transform);
00168         }
00169         catch (tf::TransformException & ex){
00170                 ROS_ERROR(" Problem %s",ex.what());
00171                 ros::Duration(1.0).sleep();
00172         }
00173 
00174 
00175 
00176         do{
00177                 /***************************************
00178                  * STEP1. PUBLISH THE VELOCITY MESSAGE
00179                  ***************************************/
00180                 velocityPublisher.publish(VelocityMessage);
00181                 ros::spinOnce();
00182                 loop_rate.sleep();
00183                 /**************************************************
00184                  * STEP2. ESTIMATE THE DISTANCE MOVED BY THE ROBOT
00185                  *************************************************/
00186                 try{
00187                         //wait for the transform to be found
00188                         listener.waitForTransform("/base_footprint", "/odom", ros::Time(0), ros::Duration(10.0) );
00189                         //Once the transform is found,get the initial_transform transformation.
00190                         listener.lookupTransform("/base_footprint", "/odom",ros::Time(0), current_transform);
00191                 }
00192                 catch (tf::TransformException & ex){
00193                         ROS_ERROR(" Problem %s",ex.what());
00194                         ros::Duration(1.0).sleep();
00195                 }
00196                 /*
00197                  * Calculate the distance moved by the robot
00198                  * There are two methods that give the same result
00199                  */
00200 
00201                 /*
00202                  * Method 1: Calculate the distance between the two transformations
00203                  * Hint:
00204                  *        --> transform.getOrigin().x(): represents the x coordinate of the transformation
00205                  *        --> transform.getOrigin().y(): represents the y coordinate of the transformation
00206                  */
00207                 //calculate the distance moved
00208                 //cout<<"Initial Transform: "<<init_transform <<" , "<<"Current Transform: "<<current_transform<<endl;
00209 
00210                 distance_moved = sqrt(pow((current_transform.getOrigin().x()-init_transform.getOrigin().x()), 2) +
00211                                 pow((current_transform.getOrigin().y()-init_transform.getOrigin().y()), 2));
00212 
00213 
00214         }while((distance_moved<distance)&&(ros::ok()));
00215         //finally, stop the robot when the distance is moved
00216         VelocityMessage.linear.x =0;
00217         velocityPublisher.publish(VelocityMessage);
00218 }
00219 
00228 void move_v2(double speed, double distance, bool isForward){
00229         //delcare a Twist message to send velocity commands
00230         geometry_msgs::Twist VelocityMessage;
00231         //declare tf transform listener: this transform listener will be used to listen and capture the transformation between
00232         // the odom frame (that represent the reference frame) and the base_footprint frame the represent moving frame
00233         tf::TransformListener listener;
00234         //declare tf transform
00235         //init_transform: is the transformation before starting the motion
00236         tf::StampedTransform init_transform;
00237         //current_transformation: is the transformation while the robot is moving
00238         tf::StampedTransform current_transform;
00239         //initial coordinates (for method 3)
00240         nav_msgs::Odometry initial_turtlebot_odom_pose;
00241 
00242         //set the linear velocity to a positive value if isFoward is true
00243         if (isForward)
00244                 VelocityMessage.linear.x =abs(speed);
00245         else //else set the velocity to negative value to move backward
00246                 VelocityMessage.linear.x =-abs(speed);
00247         //all velocities of other axes must be zero.
00248         VelocityMessage.linear.y = VelocityMessage.linear.z =VelocityMessage.angular.x =VelocityMessage.angular.y =VelocityMessage.angular.z =0;
00249 
00250         double distance_moved = 0.0;
00251         ros::Rate loop_rate(10); // we publish the velocity at 10 Hz (10 times a second)
00252 
00253         /*
00254          * First, we capture the initial transformation before starting the motion.
00255          * we call this transformation "init_transform"
00256          * It is important to "waitForTransform" otherwise, it might not be captured.
00257          */
00258         try{
00259                 //wait for the transform to be found
00260                 listener.waitForTransform("/base_footprint", "/odom", ros::Time(0), ros::Duration(10.0) );
00261                 //Once the transform is found,get the initial_transform transformation.
00262                 listener.lookupTransform("/base_footprint", "/odom",ros::Time(0), init_transform);
00263         }
00264         catch (tf::TransformException & ex){
00265                 ROS_ERROR(" Problem %s",ex.what());
00266                 ros::Duration(1.0).sleep();
00267         }
00268 
00269 
00270 
00271         do{
00272                 /***************************************
00273                  * STEP1. PUBLISH THE VELOCITY MESSAGE
00274                  ***************************************/
00275                 velocityPublisher.publish(VelocityMessage);
00276                 ros::spinOnce();
00277                 loop_rate.sleep();
00278                 /**************************************************
00279                  * STEP2. ESTIMATE THE DISTANCE MOVED BY THE ROBOT
00280                  *************************************************/
00281                 try{
00282                         //wait for the transform to be found
00283                         listener.waitForTransform("/base_footprint", "/odom", ros::Time(0), ros::Duration(10.0) );
00284                         //Once the transform is found,get the initial_transform transformation.
00285                         listener.lookupTransform("/base_footprint", "/odom",ros::Time(0), current_transform);
00286                 }
00287                 catch (tf::TransformException & ex){
00288                         ROS_ERROR(" Problem %s",ex.what());
00289                         ros::Duration(1.0).sleep();
00290                 }
00291 
00292                 /*
00293                  * Method 2: using transform composition. We calculate the relative transform, then we determine its length
00294                  * Hint:
00295                  *        --> transform.getOrigin().length(): return the displacement of the origin of the transformation
00296                  */
00297                 tf::Transform relative_transform = init_transform.inverse() * current_transform;
00298                 distance_moved= relative_transform.getOrigin().length();
00299 
00300                 //cout<<"Method 2: distance moved: "<<distance_moved <<", "<<distance<<endl;
00301 
00302         }while((distance_moved<distance)&&(ros::ok()));
00303         //finally, stop the robot when the distance is moved
00304         VelocityMessage.linear.x =0;
00305         velocityPublisher.publish(VelocityMessage);
00306 }
00307 
00308 
00317 void move_v3(double speed, double distance, bool isForward){
00318         //delcare a Twist message to send velocity commands
00319         geometry_msgs::Twist VelocityMessage;
00320         //initial pose of the turtlebot before start moving
00321         nav_msgs::Odometry initial_turtlebot_odom_pose;
00322 
00323         //set the linear velocity to a positive value if isFoward is true
00324         if (isForward)
00325                 VelocityMessage.linear.x =abs(speed);
00326         else //else set the velocity to negative value to move backward
00327                 VelocityMessage.linear.x =-abs(speed);
00328         //all velocities of other axes must be zero.
00329         VelocityMessage.linear.y = VelocityMessage.linear.z =VelocityMessage.angular.x =VelocityMessage.angular.y =VelocityMessage.angular.z =0;
00330 
00331         double distance_moved = 0.0;
00332         ros::Rate loop_rate(10); // we publish the velocity at 10 Hz (10 times a second)
00333 
00334         //we update the initial_turtlebot_odom_pose using the turtlebot_odom_pose global variable updated in the callback function poseCallback
00335         initial_turtlebot_odom_pose = turtlebot_odom_pose;
00336 
00337         do{
00338                 velocityPublisher.publish(VelocityMessage);
00339                 ros::spinOnce();
00340                 loop_rate.sleep();
00341                 distance_moved = sqrt(pow((turtlebot_odom_pose.pose.pose.position.x-initial_turtlebot_odom_pose.pose.pose.position.x), 2) +
00342                                 pow((turtlebot_odom_pose.pose.pose.position.y-initial_turtlebot_odom_pose.pose.pose.position.y), 2));
00343 
00344         }while((distance_moved<distance)&&(ros::ok()));
00345         //finally, stop the robot when the distance is moved
00346         VelocityMessage.linear.x =0;
00347         velocityPublisher.publish(VelocityMessage);
00348 }
00349 
00350 
00351 
00352 double rotate(double angular_velocity, double radians,  bool clockwise)
00353 {
00354 
00355         //delcare a Twist message to send velocity commands
00356         geometry_msgs::Twist VelocityMessage;
00357         //declare tf transform listener: this transform listener will be used to listen and capture the transformation between
00358         // the odom frame (that represent the reference frame) and the base_footprint frame the represent moving frame
00359         tf::TransformListener TFListener;
00360         //declare tf transform
00361         //init_transform: is the transformation before starting the motion
00362         tf::StampedTransform init_transform;
00363         //current_transformation: is the transformation while the robot is moving
00364         tf::StampedTransform current_transform;
00365         //initial coordinates (for method 3)
00366         nav_msgs::Odometry initial_turtlebot_odom_pose;
00367 
00368         double angle_turned =0.0;
00369 
00370         //validate angular velocity; ANGULAR_VELOCITY_MINIMUM_THRESHOLD is the minimum allowed
00371         angular_velocity=((angular_velocity>ANGULAR_VELOCITY_MINIMUM_THRESHOLD)?angular_velocity:ANGULAR_VELOCITY_MINIMUM_THRESHOLD);
00372 
00373         while(radians < 0) radians += 2*M_PI;
00374         while(radians > 2*M_PI) radians -= 2*M_PI;
00375 
00376         //wait for the listener to get the first message
00377         TFListener.waitForTransform("base_footprint", "odom", ros::Time(0), ros::Duration(1.0));
00378 
00379 
00380         //record the starting transform from the odometry to the base frame
00381         TFListener.lookupTransform("base_footprint", "odom", ros::Time(0), init_transform);
00382 
00383 
00384         //the command will be to turn at 0.75 rad/s
00385         VelocityMessage.linear.x = VelocityMessage.linear.y = 0.0;
00386         VelocityMessage.angular.z = angular_velocity;
00387         if (clockwise) VelocityMessage.angular.z = -VelocityMessage.angular.z;
00388 
00389         //the axis we want to be rotating by
00390         tf::Vector3 desired_turn_axis(0,0,1);
00391         if (!clockwise) desired_turn_axis = -desired_turn_axis;
00392 
00393         ros::Rate rate(10.0);
00394         bool done = false;
00395         while (!done )
00396         {
00397                 //send the drive command
00398                 velocityPublisher.publish(VelocityMessage);
00399                 rate.sleep();
00400                 //get the current transform
00401                 try
00402                 {
00403                         TFListener.waitForTransform("base_footprint", "odom", ros::Time(0), ros::Duration(1.0));
00404                         TFListener.lookupTransform("base_footprint", "odom", ros::Time(0), current_transform);
00405                 }
00406                 catch (tf::TransformException ex)
00407                 {
00408                         ROS_ERROR("%s",ex.what());
00409                         break;
00410                 }
00411                 tf::Transform relative_transform = init_transform.inverse() * current_transform;
00412                 tf::Vector3 actual_turn_axis = relative_transform.getRotation().getAxis();
00413                 angle_turned = relative_transform.getRotation().getAngle();
00414 
00415                 if (fabs(angle_turned) < 1.0e-2) continue;
00416                 if (actual_turn_axis.dot(desired_turn_axis ) < 0 )
00417                         angle_turned = 2 * M_PI - angle_turned;
00418 
00419                 if (!clockwise)
00420                         VelocityMessage.angular.z = (angular_velocity-ANGULAR_VELOCITY_MINIMUM_THRESHOLD) * (fabs(radian2degree(radians-angle_turned)/radian2degree(radians)))+ANGULAR_VELOCITY_MINIMUM_THRESHOLD;
00421                 else
00422                         if (clockwise)
00423                                 VelocityMessage.angular.z = (-angular_velocity+ANGULAR_VELOCITY_MINIMUM_THRESHOLD) * (fabs(radian2degree(radians-angle_turned)/radian2degree(radians)))-ANGULAR_VELOCITY_MINIMUM_THRESHOLD;
00424 
00425                 if (angle_turned > radians) {
00426                         done = true;
00427                         VelocityMessage.linear.x = VelocityMessage.linear.y = VelocityMessage.angular.z = 0;
00428                         velocityPublisher.publish(VelocityMessage);
00429                 }
00430 
00431 
00432         }
00433         if (done) return angle_turned;
00434         return angle_turned;
00435 }
00436 
00437 
00438 double calculateYaw( double x1, double y1, double x2,double y2)
00439 {
00440 
00441         double bearing = atan2((y2 - y1),(x2 - x1));
00442         //if(bearing < 0) bearing += 2 * PI;
00443         bearing *= 180.0 / M_PI;
00444         return bearing;
00445 }
00446 
00447 /* makes conversion from radian to degree */
00448 double radian2degree(double radianAngle){
00449         return (radianAngle*57.2957795);
00450 }
00451 
00452 
00453 /* makes conversion from degree to radian */
00454 double degree2radian(double degreeAngle){
00455         return (degreeAngle/57.2957795);
00456 }
00457 
00458 
00459 
00460 
00461 


gapter
Author(s):
autogenerated on Thu Jun 6 2019 22:05:13