git commit -m "first commit"

navigations/twist_recovery/CHANGELOG.rst

@@ -0,0 +1,39 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package twist_recovery
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.4.1 (2022-08-24)
+------------------
+
+0.4.0 (2022-03-07)
+------------------
+
+0.3.4 (2020-06-19)
+------------------
+* Initial release into noetic
+* Set cmake_policy CMP0048 to fix warning
+* Contributors: Martin Günther
+
+0.3.3 (2019-10-15)
+------------------
+* Add READMEs
+* Contributors: Martin Günther
+
+0.3.2 (2019-01-16)
+------------------
+
+0.3.1 (2018-09-05)
+------------------
+* twist_recovery: Add missing dependency
+  Actually, this is a workaround for the following bug: https://github.com/ros/geometry2/issues/275
+* Contributors: Martin Günther
+
+0.3.0 (2018-09-04)
+------------------
+* Convert to TF2 + new navigation API (for melodic)
+* Use non deprecated pluginlib macro + headers
+* Contributors: Martin Günther
+
+0.2.0 (2018-09-03)
+------------------
+* Initial release into indigo, kinetic, lunar and melodic

navigations/twist_recovery/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.5.1)
+cmake_policy(SET CMP0048 NEW)
+project(twist_recovery)
+
+# Find ROS dependencies
+set(THIS_PACKAGE_ROS_DEPS nav_core costmap_2d geometry_msgs pluginlib base_local_planner tf2_geometry_msgs tf2_ros)
+find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS})
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES twist_recovery
+  CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS}
+)
+
+add_library(twist_recovery src/twist_recovery.cpp)
+target_link_libraries(twist_recovery ${catkin_LIBRARIES})
+
+install(TARGETS twist_recovery
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)
+
+install(FILES twist_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

navigations/twist_recovery/README.md

@@ -0,0 +1,4 @@
+twist_recovery
+==============
+
+A recovery behavior that performs a particular used-defined twist.

navigations/twist_recovery/config/backwards.yaml

@@ -0,0 +1,5 @@
+# Configuration for version of this recovery where we back up slowly about half a meter
+linear_x: -0.3
+linear_y: 0.0
+angular_z: 0.0
+duration: 1.5

navigations/twist_recovery/config/diagonal_left.yaml

@@ -0,0 +1,4 @@
+linear_x: 0.0
+linear_y: 0.2
+angular_z: 0.0
+duration: 1.0

navigations/twist_recovery/config/diagonal_right.yaml

@@ -0,0 +1,4 @@
+linear_x: 0.0
+linear_y: -0.2
+angular_z: 0.0
+duration: 1.0

navigations/twist_recovery/include/twist_recovery/twist_recovery.h

@@ -0,0 +1,107 @@
+/*
+ * 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 
+ * 
+ * Recovery behavior based on executing a particular twist
+ *
+ * \author Bhaskara Marthi
+ */
+
+#ifndef TWIST_RECOVERY_TWIST_RECOVERY_H
+#define TWIST_RECOVERY_TWIST_RECOVERY_H
+
+#include <nav_core/recovery_behavior.h>
+#include <base_local_planner/costmap_model.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <tf2_ros/buffer.h>
+
+namespace twist_recovery
+{
+
+/// Recovery behavior that takes a given twist and tries to execute it for up to
+/// d seconds, or until reaching an obstacle.  
+class TwistRecovery : public nav_core::RecoveryBehavior
+{
+public:
+  
+  /// Doesn't do anything: initialize is where the actual work happens
+  TwistRecovery();
+
+  ~TwistRecovery();
+
+  /// Initialize the parameters of the behavior
+  void initialize (std::string n, tf2_ros::Buffer* tf,
+                   costmap_2d::Costmap2DROS* global_costmap,
+                   costmap_2d::Costmap2DROS* local_costmap);
+
+  /// Run the behavior
+  void runBehavior();
+
+private:
+
+  geometry_msgs::Pose2D getCurrentLocalPose () const;
+  geometry_msgs::Twist scaleGivenAccelerationLimits (const geometry_msgs::Twist& twist, const double time_remaining) const;
+  double nonincreasingCostInterval (const geometry_msgs::Pose2D& current, const geometry_msgs::Twist& twist) const;
+  double normalizedPoseCost (const geometry_msgs::Pose2D& pose) const;
+  geometry_msgs::Twist transformTwist (const geometry_msgs::Pose2D& pose) const;
+
+  ros::NodeHandle nh_;
+  costmap_2d::Costmap2DROS* global_costmap_;
+  costmap_2d::Costmap2DROS* local_costmap_;
+  std::string name_;
+  tf2_ros::Buffer* tf_;
+  ros::Publisher pub_;
+  bool initialized_;
+
+  // Memory owned by this object
+  // Mutable because footprintCost is not declared const
+  mutable base_local_planner::CostmapModel* world_model_;
+
+  geometry_msgs::Twist base_frame_twist_;
+  
+  double duration_;
+  double linear_speed_limit_;
+  double angular_speed_limit_;
+  double linear_acceleration_limit_;
+  double angular_acceleration_limit_;
+  double controller_frequency_;
+  double simulation_inc_;
+  
+  
+};
+
+} // namespace twist_recovery
+
+#endif // include guard

navigations/twist_recovery/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>twist_recovery</name>
+  <version>0.4.1</version>
+  <description>
+    A recovery behavior that performs a particular used-defined twist.
+  </description>
+  <maintainer email="martin.guenther@dfki.de">Martin Günther</maintainer>
+  <author>Bhaskara Marthi</author>
+  <license>BSD</license>
+  <url type="website">http://wiki.ros.org/twist_recovery</url>
+  <url type="repository">https://github.com/ros-planning/navigation_experimental.git</url>
+  <url type="bugtracker">https://github.com/ros-planning/navigation_experimental/issues</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>base_local_planner</depend>
+  <depend>costmap_2d</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_core</depend>
+  <depend>pluginlib</depend>
+  <depend>tf2_geometry_msgs</depend>  <!-- required whenever tf2/utils.h is included: https://github.com/ros/geometry2/issues/275 -->
+  <depend>tf2_ros</depend>
+
+  <export>
+    <nav_core plugin="${prefix}/twist_plugin.xml" />
+  </export>
+</package>

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
+

navigations/twist_recovery/twist_plugin.xml

@@ -0,0 +1,7 @@
+<library path="lib/libtwist_recovery">
+  <class name="twist_recovery/TwistRecovery" type="twist_recovery::TwistRecovery" base_class_type="nav_core::RecoveryBehavior">
+    <description>
+      A recovery behavior that performs a particular used-defined twist.
+    </description>
+  </class>
+</library>