git commit -m "first commit"

navigations/twist_recovery/src/twist_recovery.cpp

@@ -0,0 +1,206 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+/**
+ * \file 
+ * 
+ * \author Bhaskara Marthi
+ * 
+ */
+
+#include <twist_recovery/twist_recovery.h>
+#include <pluginlib/class_list_macros.hpp>
+#include <tf/transform_datatypes.h>
+#include <tf2/utils.h>
+
+// register as a RecoveryBehavior plugin
+PLUGINLIB_EXPORT_CLASS(twist_recovery::TwistRecovery, nav_core::RecoveryBehavior)
+
+namespace gm=geometry_msgs;
+namespace cmap=costmap_2d;
+namespace blp=base_local_planner;
+using std::vector;
+using std::max;
+
+namespace twist_recovery
+{
+
+TwistRecovery::TwistRecovery () :
+  global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
+{}
+
+TwistRecovery::~TwistRecovery ()
+{
+  delete world_model_;
+}
+
+void TwistRecovery::initialize (std::string name, tf2_ros::Buffer* tf,
+                                cmap::Costmap2DROS* global_cmap, cmap::Costmap2DROS* local_cmap)
+{
+  ROS_ASSERT(!initialized_);
+  name_ = name;
+  tf_ = tf;
+  local_costmap_ = local_cmap;
+  global_costmap_ = global_cmap;
+  world_model_ = new blp::CostmapModel(*local_costmap_->getCostmap());
+
+  pub_ = nh_.advertise<gm::Twist>("cmd_vel", 10);
+  ros::NodeHandle private_nh("~/" + name);
+
+  {
+  bool found=true;
+  found = found && private_nh.getParam("linear_x", base_frame_twist_.linear.x);
+  found = found && private_nh.getParam("linear_y", base_frame_twist_.linear.y);
+  found = found && private_nh.getParam("angular_z", base_frame_twist_.angular.z);
+  if (!found) {
+    ROS_FATAL_STREAM ("Didn't find twist parameters in " << private_nh.getNamespace());
+    ros::shutdown();
+  }
+  }
+
+  private_nh.param("duration", duration_, 1.0);
+  private_nh.param("linear_speed_limit", linear_speed_limit_, 0.3);
+  private_nh.param("angular_speed_limit", angular_speed_limit_, 1.0);
+  private_nh.param("linear_acceleration_limit", linear_acceleration_limit_, 4.0);
+  private_nh.param("angular_acceleration_limit", angular_acceleration_limit_, 3.2);
+  private_nh.param("controller_frequency", controller_frequency_, 20.0);
+  private_nh.param("simulation_inc", simulation_inc_, 1/controller_frequency_);
+
+  ROS_INFO_STREAM_NAMED ("top", "Initialized twist recovery with twist " <<
+                          base_frame_twist_ << " and duration " << duration_);
+  
+  initialized_ = true;
+}
+
+gm::Twist scaleTwist (const gm::Twist& twist, const double scale)
+{
+  gm::Twist t;
+  t.linear.x = twist.linear.x * scale;
+  t.linear.y = twist.linear.y * scale;
+  t.angular.z = twist.angular.z * scale;
+  return t;
+}
+
+gm::Pose2D forwardSimulate (const gm::Pose2D& p, const gm::Twist& twist, const double t=1.0)
+{
+  gm::Pose2D p2;
+  p2.x = p.x + twist.linear.x*t;
+  p2.y = p.y + twist.linear.y*t;
+  p2.theta = p.theta + twist.angular.z*t;
+  return p2;
+}
+
+/// Return the cost of a pose, modified so that -1 does not equal infinity; instead 1e9 does.
+double TwistRecovery::normalizedPoseCost (const gm::Pose2D& pose) const
+{
+  const double c = world_model_->footprintCost(pose.x, pose.y, pose.theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
+  return c < 0 ? 1e9 : c;
+}
+
+
+/// Return the maximum d <= duration_ such that starting at the current pose, the cost is nonincreasing for
+/// d seconds if we follow twist
+/// It might also be good to have a threshold such that we're allowed to have lethal cost for at most
+/// the first k of those d seconds, but this is not done
+double TwistRecovery::nonincreasingCostInterval (const gm::Pose2D& current, const gm::Twist& twist) const
+{
+  double cost = normalizedPoseCost(current);
+  double t; // Will hold the first time that is invalid
+  for (t=simulation_inc_; t<=duration_; t+=simulation_inc_) {
+    const double next_cost = normalizedPoseCost(forwardSimulate(current, twist, t));
+    if (next_cost > cost) {
+      ROS_DEBUG_STREAM_NAMED ("cost", "Cost at " << t << " and pose " << forwardSimulate(current, twist, t)
+                              << " is " << next_cost << " which is greater than previous cost " << cost);
+      break;
+    }
+    cost = next_cost;
+  }
+  
+  return t-simulation_inc_;
+}
+
+double linearSpeed (const gm::Twist& twist)
+{
+  return sqrt(twist.linear.x*twist.linear.x + twist.linear.y*twist.linear.y);
+}
+
+double angularSpeed (const gm::Twist& twist)
+{
+  return fabs(twist.angular.z);
+}
+
+// Scale twist so we can stop in the given time, and so it's within the max velocity
+gm::Twist TwistRecovery::scaleGivenAccelerationLimits (const gm::Twist& twist, const double time_remaining) const
+{
+  const double linear_speed = linearSpeed(twist);
+  const double angular_speed = angularSpeed(twist);
+  const double linear_acc_scaling = linear_speed/(time_remaining*linear_acceleration_limit_);
+  const double angular_acc_scaling = angular_speed/(time_remaining*angular_acceleration_limit_);
+  const double acc_scaling = max(linear_acc_scaling, angular_acc_scaling);
+  const double linear_vel_scaling = linear_speed/linear_speed_limit_;
+  const double angular_vel_scaling = angular_speed/angular_speed_limit_;
+  const double vel_scaling = max(linear_vel_scaling, angular_vel_scaling);
+  return scaleTwist(twist, max(1.0, max(acc_scaling, vel_scaling)));
+}
+
+// Get pose in local costmap frame
+gm::Pose2D TwistRecovery::getCurrentLocalPose () const
+{
+  gm::PoseStamped p;
+  local_costmap_->getRobotPose(p);
+  gm::Pose2D pose;
+  pose.x = p.pose.position.x;
+  pose.y = p.pose.position.y;
+  pose.theta = tf2::getYaw(p.pose.orientation);
+  return pose;
+}
+
+void TwistRecovery::runBehavior ()
+{
+  ROS_ASSERT (initialized_);
+
+  // Figure out how long we can safely run the behavior
+  const gm::Pose2D& current = getCurrentLocalPose();
+  
+  const double d = nonincreasingCostInterval(current, base_frame_twist_);
+  ros::Rate r(controller_frequency_);
+  ROS_WARN_NAMED ("top", "Applying (%.2f, %.2f, %.2f) for %.2f seconds", base_frame_twist_.linear.x,
+                   base_frame_twist_.linear.y, base_frame_twist_.angular.z, d);
+                   
+  // We'll now apply this twist open-loop for d seconds (scaled so we can guarantee stopping at the end)
+  for (double t=0; t<d; t+=1/controller_frequency_) {
+    pub_.publish(scaleGivenAccelerationLimits(base_frame_twist_, d-t));
+    r.sleep();
+  }    
+}
+
+
+} // namespace twist_recovery
+