eband_action.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, 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 
00041 #include <eband_local_planner/FollowBaseTrajectoryAction.h>
00042 #include <actionlib/server/simple_action_server.h>
00043 #include <geometry_msgs/Twist.h>
00044 #include <tf/transform_listener.h>
00045 #include <eband_local_planner/eband_local_planner_ros.h>
00046 #include <boost/thread.hpp>
00047 #include <boost/foreach.hpp>
00048 #include <boost/format.hpp>
00049 
00050 namespace al=actionlib;
00051 namespace gm=geometry_msgs;
00052 namespace cmap=costmap_2d;
00053 namespace eband=eband_local_planner;
00054 
00055 using std::string;
00056 using boost::format;
00057 
00058 namespace eband_local_planner
00059 {
00060 
00061 typedef FollowBaseTrajectoryGoalConstPtr GoalPtr;
00062 typedef FollowBaseTrajectoryResult Result;
00063 typedef boost::mutex::scoped_lock Lock;
00064 
00065 Result result (const string& msg)
00066 {
00067   Result r;
00068   r.msg = msg;
00069   return r;
00070 }
00071 
00072 Result result (const format& f)
00073 {
00074   return result(f.str());
00075 }
00076 
00077 // Activates costmap within this scope and pauses it when leaving
00078 struct ScopedCostmapActivate
00079 {
00080   ScopedCostmapActivate (cmap::Costmap2DROS* cmap) : cmap(cmap)
00081   {
00082     cmap->start();
00083   }
00084 
00085   ~ScopedCostmapActivate ()
00086   {
00087     cmap->pause();
00088   }
00089 
00090   cmap::Costmap2DROS* cmap;
00091 };
00092 
00093 // Holds ros node state
00094 class Node
00095 {
00096 public:
00097   
00098   Node ();
00099   void execute (const GoalPtr& goal);
00100 
00101 private:
00102 
00103   ros::Rate control_rate_;
00104   ros::Duration control_timeout_;
00105 
00106   ros::Time last_valid_control_;
00107   
00108   boost::mutex mutex_;
00109   ros::NodeHandle nh_;
00110   tf::TransformListener tf_;
00111   al::SimpleActionServer<FollowBaseTrajectoryAction> as_;
00112   cmap::Costmap2DROS costmap_ros_;
00113   ros::Publisher vel_pub_;
00114   eband::EBandPlannerROS tc_;
00115 
00116 };
00117 
00118 inline gm::Twist zeroVelocity ()
00119 {
00120   return gm::Twist();
00121 }
00122 
00123 
00124 Node::Node () :
00125   control_rate_(10.0), control_timeout_(2.0),
00126   as_(nh_, "follow_base_trajectory", boost::bind(&Node::execute, this, _1), false),
00127   costmap_ros_("elastic_band_planner", tf_),
00128   vel_pub_(nh_.advertise<gm::Twist>("cmd_vel", 10))
00129 {
00130   Lock l(mutex_);
00131   costmap_ros_.pause();
00132   
00133   // Initialize the 'trajectory controller' that will do the work
00134   tc_.initialize("elastic_band_planner", &tf_, &costmap_ros_);
00135   as_.start();
00136   ROS_INFO ("Eband node initialized");
00137 }
00138 
00139 void Node::execute (const GoalPtr& goal)
00140 {
00141   // Make it so costmap is active within body of this function
00142   ScopedCostmapActivate cmap_activate(&costmap_ros_);
00143   Lock l(mutex_);
00144 
00145   const unsigned n=goal->path.size();
00146   if (n<2)
00147   {
00148     as_.setAborted(result(format("Path had length %1%") % n));
00149     return;
00150   }
00151   // Now guaranteed path has length > 1
00152   
00153   ROS_DEBUG_STREAM_NAMED ("control", "Received trajectory of length " << n <<
00154                           " going from " << goal->path[0].pose << " to "
00155                           << goal->path[n-1].pose);
00156   if (!tc_.setPlan(goal->path))
00157   {
00158     as_.setAborted(result("Failed setting plan for controller;"
00159                           "see stdout for error message"));
00160     return;
00161   }
00162   // Now guaranteed that controller plan was set correctly
00163 
00164   // Main loop
00165   last_valid_control_ = ros::Time::now();
00166   while (ros::ok() && !tc_.isGoalReached())
00167   {
00168     gm::Twist vel;
00169     if (!tc_.computeVelocityCommands(vel))
00170     {
00171       ROS_DEBUG_STREAM_NAMED ("control", "Didn't find valid control");
00172       vel_pub_.publish(zeroVelocity());
00173       if (ros::Time::now() - last_valid_control_ > control_timeout_)
00174       {
00175         as_.setAborted(result("Timed out finding a valid control"));
00176         return;
00177       }
00178       continue;
00179     }
00180 
00181     // Successfully got command
00182     ROS_DEBUG_STREAM_THROTTLE_NAMED (1.0, "control", "Sending control " << vel);
00183     vel_pub_.publish(vel);
00184     last_valid_control_ = ros::Time::now();
00185     ros::spinOnce();
00186     control_rate_.sleep();
00187   }
00188 
00189   ROS_DEBUG_STREAM_NAMED ("control", "Succeeded");
00190   as_.setSucceeded();
00191 }
00192 
00193 } // namespace
00194 
00195 int main (int argc, char** argv)
00196 {
00197   ros::init(argc, argv, "eband_action");
00198   ros::NodeHandle nh;
00199   eband_local_planner::Node node;
00200   ros::spin();
00201   return 0;
00202 }


eband_local_planner
Author(s): Christian Connette, Bhaskara Marthi
autogenerated on Fri Jan 3 2014 11:34:16