twist_recovery.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 
00038 #include <twist_recovery/twist_recovery.h>
00039 #include <pluginlib/class_list_macros.h>
00040 #include <tf/transform_datatypes.h>
00041 
00042 // register as a RecoveryBehavior plugin
00043 PLUGINLIB_DECLARE_CLASS(twist_recovery, TwistRecovery, twist_recovery::TwistRecovery,      
00044                         nav_core::RecoveryBehavior)
00045 
00046 namespace gm=geometry_msgs;
00047 namespace cmap=costmap_2d;
00048 namespace blp=base_local_planner;
00049 using std::vector;
00050 using std::max;
00051 
00052 namespace twist_recovery
00053 {
00054 
00055 TwistRecovery::TwistRecovery () :
00056   global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
00057 {}
00058 
00059 TwistRecovery::~TwistRecovery ()
00060 {
00061   delete world_model_;
00062 }
00063 
00064 void TwistRecovery::initialize (std::string name, tf::TransformListener* tf,
00065                                 cmap::Costmap2DROS* global_cmap, cmap::Costmap2DROS* local_cmap)
00066 {
00067   ROS_ASSERT(!initialized_);
00068   name_ = name;
00069   tf_ = tf;
00070   local_costmap_ = local_cmap;
00071   global_costmap_ = global_cmap;
00072   local_costmap_->getCostmapCopy(costmap_);
00073   world_model_ = new blp::CostmapModel(costmap_);
00074 
00075   pub_ = nh_.advertise<gm::Twist>("cmd_vel", 10);
00076   ros::NodeHandle private_nh("~/" + name);
00077 
00078   {
00079   bool found=true;
00080   found = found && private_nh.getParam("linear_x", base_frame_twist_.linear.x);
00081   found = found && private_nh.getParam("linear_y", base_frame_twist_.linear.y);
00082   found = found && private_nh.getParam("angular_z", base_frame_twist_.angular.z);
00083   if (!found) {
00084     ROS_FATAL_STREAM ("Didn't find twist parameters in " << private_nh.getNamespace());
00085     ros::shutdown();
00086   }
00087   }
00088 
00089   private_nh.param("duration", duration_, 1.0);
00090   private_nh.param("linear_speed_limit", linear_speed_limit_, 0.3);
00091   private_nh.param("angular_speed_limit", angular_speed_limit_, 1.0);
00092   private_nh.param("linear_acceleration_limit", linear_acceleration_limit_, 4.0);
00093   private_nh.param("angular_acceleration_limit", angular_acceleration_limit_, 3.2);
00094   private_nh.param("controller_frequency", controller_frequency_, 20.0);
00095   private_nh.param("simulation_inc", simulation_inc_, 1/controller_frequency_);
00096 
00097   ROS_INFO_STREAM_NAMED ("top", "Initialized twist recovery with twist " <<
00098                           base_frame_twist_ << " and duration " << duration_);
00099   
00100   initialized_ = true;
00101 }
00102 
00103 gm::Twist scaleTwist (const gm::Twist& twist, const double scale)
00104 {
00105   gm::Twist t;
00106   t.linear.x = twist.linear.x * scale;
00107   t.linear.y = twist.linear.y * scale;
00108   t.angular.z = twist.angular.z * scale;
00109   return t;
00110 }
00111 
00112 gm::Pose2D forwardSimulate (const gm::Pose2D& p, const gm::Twist& twist, const double t=1.0)
00113 {
00114   gm::Pose2D p2;
00115   p2.x = p.x + twist.linear.x*t;
00116   p2.y = p.y + twist.linear.y*t;
00117   p2.theta = p.theta + twist.angular.z*t;
00118   return p2;
00119 }
00120 
00122 double TwistRecovery::normalizedPoseCost (const gm::Pose2D& pose) const
00123 {
00124   gm::Point p;
00125   p.x = pose.x;
00126   p.y = pose.y;
00127   vector<gm::Point> oriented_footprint;
00128   local_costmap_->getOrientedFootprint(pose.x, pose.y, pose.theta, oriented_footprint);
00129   const double c = world_model_->footprintCost(p, oriented_footprint, local_costmap_->getInscribedRadius(),
00130                                                local_costmap_->getCircumscribedRadius());
00131   return c < 0 ? 1e9 : c;
00132 }
00133 
00134 
00139 double TwistRecovery::nonincreasingCostInterval (const gm::Pose2D& current, const gm::Twist& twist) const
00140 {
00141   double cost = normalizedPoseCost(current);
00142   double t; // Will hold the first time that is invalid
00143   for (t=simulation_inc_; t<=duration_; t+=simulation_inc_) {
00144     const double next_cost = normalizedPoseCost(forwardSimulate(current, twist, t));
00145     if (next_cost > cost) {
00146       ROS_DEBUG_STREAM_NAMED ("cost", "Cost at " << t << " and pose " << forwardSimulate(current, twist, t)
00147                               << " is " << next_cost << " which is greater than previous cost " << cost);
00148       break;
00149     }
00150     cost = next_cost;
00151   }
00152   
00153   return t-simulation_inc_;
00154 }
00155 
00156 double linearSpeed (const gm::Twist& twist)
00157 {
00158   return sqrt(twist.linear.x*twist.linear.x + twist.linear.y*twist.linear.y);
00159 }
00160 
00161 double angularSpeed (const gm::Twist& twist)
00162 {
00163   return fabs(twist.angular.z);
00164 }
00165 
00166 // Scale twist so we can stop in the given time, and so it's within the max velocity
00167 gm::Twist TwistRecovery::scaleGivenAccelerationLimits (const gm::Twist& twist, const double time_remaining) const
00168 {
00169   const double linear_speed = linearSpeed(twist);
00170   const double angular_speed = angularSpeed(twist);
00171   const double linear_acc_scaling = linear_speed/(time_remaining*linear_acceleration_limit_);
00172   const double angular_acc_scaling = angular_speed/(time_remaining*angular_acceleration_limit_);
00173   const double acc_scaling = max(linear_acc_scaling, angular_acc_scaling);
00174   const double linear_vel_scaling = linear_speed/linear_speed_limit_;
00175   const double angular_vel_scaling = angular_speed/angular_speed_limit_;
00176   const double vel_scaling = max(linear_vel_scaling, angular_vel_scaling);
00177   return scaleTwist(twist, max(1.0, max(acc_scaling, vel_scaling)));
00178 }
00179 
00180 // Get pose in local costmap frame
00181 gm::Pose2D TwistRecovery::getCurrentLocalPose () const
00182 {
00183   tf::Stamped<tf::Pose> p;
00184   local_costmap_->getRobotPose(p);
00185   gm::Pose2D pose;
00186   pose.x = p.getOrigin().x();
00187   pose.y = p.getOrigin().y();
00188   pose.theta = tf::getYaw(p.getRotation());
00189   return pose;
00190 }
00191 
00192 void TwistRecovery::runBehavior ()
00193 {
00194   ROS_ASSERT (initialized_);
00195 
00196   // Figure out how long we can safely run the behavior
00197   const gm::Pose2D& current = getCurrentLocalPose();
00198   local_costmap_->getCostmapCopy(costmap_); // This affects world_model_, which is used in the next step
00199   
00200   const double d = nonincreasingCostInterval(current, base_frame_twist_);
00201   ros::Rate r(controller_frequency_);
00202   ROS_INFO_NAMED ("top", "Applying (%.2f, %.2f, %.2f) for %.2f seconds", base_frame_twist_.linear.x,
00203                    base_frame_twist_.linear.y, base_frame_twist_.angular.z, d);
00204                    
00205   // We'll now apply this twist open-loop for d seconds (scaled so we can guarantee stopping at the end)
00206   for (double t=0; t<d; t+=1/controller_frequency_) {
00207     pub_.publish(scaleGivenAccelerationLimits(base_frame_twist_, d-t));
00208     r.sleep();
00209   }    
00210 }
00211 
00212 
00213 } // namespace twist_recovery
00214 


twist_recovery
Author(s): Bhaskara Marthi
autogenerated on Mon Oct 6 2014 02:48:30