turtlebot_move_action_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 #include <ros/ros.h>
00032 #include <std_msgs/Float32.h>
00033 #include <actionlib/server/simple_action_server.h>
00034 #include <turtlebot_actions/TurtlebotMoveAction.h>
00035 #include <geometry_msgs/Twist.h>
00036 #include <tf/transform_listener.h>
00037 #include <cmath>
00038 
00039 class MoveActionServer
00040 {
00041 private:
00042     
00043   ros::NodeHandle nh_;
00044   actionlib::SimpleActionServer<turtlebot_actions::TurtlebotMoveAction> as_;
00045   std::string action_name_;
00046 
00047   turtlebot_actions::TurtlebotMoveFeedback     feedback_;
00048   turtlebot_actions::TurtlebotMoveResult       result_;
00049   turtlebot_actions::TurtlebotMoveGoalConstPtr goal_;
00050   
00051   ros::Subscriber       sub_;
00052   ros::Publisher        cmd_vel_pub_;
00053   tf::TransformListener listener_;
00054   
00055   // Parameters
00056   std::string base_frame;
00057   std::string odom_frame;
00058   double turn_rate;
00059   double forward_rate;
00060   
00061 public:
00062   MoveActionServer(const std::string name) : 
00063     nh_("~"), as_(nh_, name, false), action_name_(name)
00064   {
00065     // Get parameters
00066     nh_.param<std::string>("base_frame", base_frame, "base_link");
00067     nh_.param<std::string>("odom_frame", odom_frame, "odom");
00068     nh_.param<double>("turn_rate", turn_rate, 0.75);
00069     nh_.param<double>("forward_rate", forward_rate, 0.25);
00070     
00071     //register the goal and feeback callbacks
00072     as_.registerGoalCallback(boost::bind(&MoveActionServer::goalCB, this));
00073     as_.registerPreemptCallback(boost::bind(&MoveActionServer::preemptCB, this));
00074     
00075     as_.start();
00076     
00077     cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00078   }
00079 
00080   void goalCB()
00081   {
00082     // accept the new goal
00083     feedback_.forward_distance = 0.0;
00084     feedback_.turn_distance = 0.0;
00085     
00086     result_.forward_distance = 0.0;
00087     result_.turn_distance = 0.0;
00088     
00089     goal_ = as_.acceptNewGoal();
00090     
00091     if (!turnOdom(goal_->turn_distance))
00092     { 
00093       as_.setAborted(result_);
00094       return;
00095     }
00096     
00097     if (driveForwardOdom(goal_->forward_distance))
00098       as_.setSucceeded(result_);
00099     else
00100       as_.setAborted(result_);
00101   }
00102 
00103   void preemptCB()
00104   {
00105     ROS_INFO("%s: Preempted", action_name_.c_str());
00106     // set the action state to preempted
00107     as_.setPreempted();
00108   }
00109 
00110   bool driveForwardOdom(double distance)
00111   {
00112     // If the distance to travel is negligble, don't even try.
00113     if (fabs(distance) < 0.01)
00114       return true;
00115     
00116     //we will record transforms here
00117     tf::StampedTransform start_transform;
00118     tf::StampedTransform current_transform;
00119   
00120     try
00121     {
00122       //wait for the listener to get the first message
00123       listener_.waitForTransform(base_frame, odom_frame, 
00124                                  ros::Time::now(), ros::Duration(1.0));
00125       
00126       //record the starting transform from the odometry to the base frame
00127       listener_.lookupTransform(base_frame, odom_frame, 
00128                                 ros::Time(0), start_transform);
00129     }
00130     catch (tf::TransformException ex)
00131     {
00132       ROS_ERROR("%s",ex.what());
00133       return false;
00134     }
00135     
00136     //we will be sending commands of type "twist"
00137     geometry_msgs::Twist base_cmd;
00138     //the command will be to go forward at 0.25 m/s
00139     base_cmd.linear.y = base_cmd.angular.z = 0;
00140     base_cmd.linear.x = forward_rate;
00141     
00142     if (distance < 0)
00143       base_cmd.linear.x = -base_cmd.linear.x;
00144     
00145     ros::Rate rate(25.0);
00146     bool done = false;
00147     while (!done && nh_.ok() && as_.isActive())
00148     {
00149       //send the drive command
00150       cmd_vel_pub_.publish(base_cmd);
00151       rate.sleep(); 
00152       //get the current transform
00153       try
00154       {
00155         listener_.lookupTransform(base_frame, odom_frame, 
00156                                   ros::Time(0), current_transform);
00157       }
00158       catch (tf::TransformException ex)
00159       {
00160         ROS_ERROR("%s",ex.what());
00161         break;
00162       }
00163       //see how far we've traveled
00164       tf::Transform relative_transform = 
00165         start_transform.inverse() * current_transform;
00166       double dist_moved = relative_transform.getOrigin().length();
00167       
00168       // Update feedback and result.
00169       feedback_.forward_distance = dist_moved;
00170       result_.forward_distance = dist_moved;
00171       as_.publishFeedback(feedback_);
00172 
00173       if(fabs(dist_moved) > fabs(distance))
00174       {
00175         done = true;
00176       }
00177     }
00178     base_cmd.linear.x = 0.0;
00179     base_cmd.angular.z = 0.0;
00180     cmd_vel_pub_.publish(base_cmd);
00181 
00182     if (done) return true;
00183     return false;
00184   }
00185 
00186   bool turnOdom(double radians)
00187   {
00188     // If the distance to travel is negligble, don't even try.
00189     if (fabs(radians) < 0.01)
00190       return true;
00191   
00192     while(radians < -M_PI) radians += 2*M_PI;
00193     while(radians > M_PI) radians -= 2*M_PI;
00194 
00195     //we will record transforms here
00196     tf::StampedTransform start_transform;
00197     tf::StampedTransform current_transform;
00198 
00199     try
00200     {
00201       //wait for the listener to get the first message
00202       listener_.waitForTransform(base_frame, odom_frame, 
00203                                  ros::Time::now(), ros::Duration(1.0));
00204 
00205       //record the starting transform from the odometry to the base frame
00206       listener_.lookupTransform(base_frame, odom_frame, 
00207                                 ros::Time(0), start_transform);
00208     }
00209     catch (tf::TransformException ex)
00210     {
00211       ROS_ERROR("%s",ex.what());
00212       return false;
00213     }
00214     
00215     //we will be sending commands of type "twist"
00216     geometry_msgs::Twist base_cmd;
00217     //the command will be to turn at 0.75 rad/s
00218     base_cmd.linear.x = base_cmd.linear.y = 0.0;
00219     base_cmd.angular.z = turn_rate;
00220     if (radians < 0)
00221       base_cmd.angular.z = -turn_rate;
00222     
00223     //the axis we want to be rotating by
00224     tf::Vector3 desired_turn_axis(0,0,1);
00225     
00226     ros::Rate rate(25.0);
00227     bool done = false;
00228     while (!done && nh_.ok() && as_.isActive())
00229     {
00230       //send the drive command
00231       cmd_vel_pub_.publish(base_cmd);
00232       rate.sleep();
00233       //get the current transform
00234       try
00235       {
00236         listener_.lookupTransform(base_frame, odom_frame, 
00237                                   ros::Time(0), current_transform);
00238       }
00239       catch (tf::TransformException ex)
00240       {
00241         ROS_ERROR("%s",ex.what());
00242         break;
00243       }
00244       tf::Transform relative_transform = 
00245         start_transform.inverse() * current_transform;
00246       tf::Vector3 actual_turn_axis = 
00247         relative_transform.getRotation().getAxis();
00248       double angle_turned = relative_transform.getRotation().getAngle();
00249       
00250       // Update feedback and result.
00251       feedback_.turn_distance = angle_turned;
00252       result_.turn_distance = angle_turned;
00253       as_.publishFeedback(feedback_);
00254       
00255       if ( fabs(angle_turned) < 1.0e-2) continue;
00256 
00257       //if ( actual_turn_axis.dot( desired_turn_axis ) < 0 ) 
00258       //  angle_turned = 2 * M_PI - angle_turned;
00259 
00260       if (fabs(angle_turned) > fabs(radians)) done = true;
00261     }
00262     if (done) return true;
00263     return false;
00264   }
00265 
00266 
00267 };
00268 
00269 int main(int argc, char** argv)
00270 {
00271   ros::init(argc, argv, "turtlebot_move_action_server");
00272 
00273   MoveActionServer server("turtlebot_move");
00274   ros::spin();
00275 
00276   return 0;
00277 }
00278 


turtlebot_actions
Author(s): Helen Oleynikova, Melonee Wise
autogenerated on Wed Aug 26 2015 16:34:28