Skip to content

Commit

Permalink
Cherry pick #914
Browse files Browse the repository at this point in the history
  • Loading branch information
@DLu
DLu committed Aug 30, 2019
1 parent 51424f9 commit b011ca0
Show file tree
Hide file tree
Showing 2 changed files with 109 additions and 89 deletions.
81 changes: 38 additions & 43 deletions rotate_recovery/include/rotate_recovery/rotate_recovery.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,58 +34,53 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef ROTATE_RECOVERY_H_
#define ROTATE_RECOVERY_H_
#ifndef ROTATE_RECOVERY_ROTATE_RECOVERY_H
#define ROTATE_RECOVERY_ROTATE_RECOVERY_H
#include <nav_core/recovery_behavior.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_ros/buffer.h>
#include <ros/ros.h>
#include <base_local_planner/costmap_model.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <angles/angles.h>
#include <string>

namespace rotate_recovery{
namespace rotate_recovery
{
/**
* @class RotateRecovery
* @brief A recovery behavior that rotates the robot in-place to attempt to clear out space
*/
class RotateRecovery : public nav_core::RecoveryBehavior
{
public:
/**
* @class RotateRecovery
* @brief A recovery behavior that rotates the robot in-place to attempt to clear out space
* @brief Constructor, make sure to call initialize in addition to actually initialize the object
*/
class RotateRecovery : public nav_core::RecoveryBehavior {
public:
/**
* @brief Constructor, make sure to call initialize in addition to actually initialize the object
* @param
* @return
*/
RotateRecovery();
RotateRecovery();

/**
* @brief Initialization function for the RotateRecovery recovery behavior
* @param tf A pointer to a transform listener
* @param global_costmap A pointer to the global_costmap used by the navigation stack
* @param local_costmap A pointer to the local_costmap used by the navigation stack
*/
void initialize(std::string name, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap);
/**
* @brief Initialization function for the RotateRecovery recovery behavior
* @param name Namespace used in initialization
* @param tf (unused)
* @param global_costmap (unused)
* @param local_costmap A pointer to the local_costmap used by the navigation stack
*/
void initialize(std::string name, tf2_ros::Buffer*,
costmap_2d::Costmap2DROS*, costmap_2d::Costmap2DROS* local_costmap);

/**
* @brief Run the RotateRecovery recovery behavior.
*/
void runBehavior();
/**
* @brief Run the RotateRecovery recovery behavior.
*/
void runBehavior();

/**
* @brief Destructor for the rotate recovery behavior
*/
~RotateRecovery();
/**
* @brief Destructor for the rotate recovery behavior
*/
~RotateRecovery();

private:
costmap_2d::Costmap2DROS* global_costmap_, *local_costmap_;
costmap_2d::Costmap2D costmap_;
std::string name_;
tf2_ros::Buffer* tf_;
bool initialized_;
double sim_granularity_, min_rotational_vel_, max_rotational_vel_, acc_lim_th_, tolerance_, frequency_;
base_local_planner::CostmapModel* world_model_;
};
private:
costmap_2d::Costmap2DROS* local_costmap_;
bool initialized_;
double sim_granularity_, min_rotational_vel_, max_rotational_vel_, acc_lim_th_, tolerance_, frequency_;
base_local_planner::CostmapModel* world_model_;
};
#endif
}; // namespace rotate_recovery
#endif // ROTATE_RECOVERY_ROTATE_RECOVERY_H
117 changes: 71 additions & 46 deletions rotate_recovery/src/rotate_recovery.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,28 +38,35 @@
#include <pluginlib/class_list_macros.h>
#include <nav_core/parameter_magic.h>
#include <tf2/utils.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <angles/angles.h>
#include <algorithm>
#include <string>

//register this planner as a RecoveryBehavior plugin
PLUGINLIB_EXPORT_CLASS(rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior)

namespace rotate_recovery {
// register this planner as a RecoveryBehavior plugin
PLUGINLIB_EXPORT_CLASS(rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior)

RotateRecovery::RotateRecovery(): global_costmap_(NULL), local_costmap_(NULL),
tf_(NULL), initialized_(false), world_model_(NULL) {}
namespace rotate_recovery
{
RotateRecovery::RotateRecovery(): local_costmap_(NULL), initialized_(false), world_model_(NULL)
{
}

void RotateRecovery::initialize(std::string name, tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){
if(!initialized_){
name_ = name;
tf_ = tf;
global_costmap_ = global_costmap;
void RotateRecovery::initialize(std::string name, tf2_ros::Buffer*,
costmap_2d::Costmap2DROS*, costmap_2d::Costmap2DROS* local_costmap)
{
if (!initialized_)
{
local_costmap_ = local_costmap;

//get some parameters from the parameter server
ros::NodeHandle private_nh("~/" + name_);
// get some parameters from the parameter server
ros::NodeHandle private_nh("~/" + name);
ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");

//we'll simulate every degree by default
// we'll simulate every degree by default
private_nh.param("sim_granularity", sim_granularity_, 0.017);
private_nh.param("frequency", frequency_, 20.0);

Expand All @@ -72,23 +79,28 @@ void RotateRecovery::initialize(std::string name, tf2_ros::Buffer* tf,

initialized_ = true;
}
else{
else
{
ROS_ERROR("You should not call initialize twice on this object, doing nothing");
}
}

RotateRecovery::~RotateRecovery(){
RotateRecovery::~RotateRecovery()
{
delete world_model_;
}

void RotateRecovery::runBehavior(){
if(!initialized_){
void RotateRecovery::runBehavior()
{
if (!initialized_)
{
ROS_ERROR("This object must be initialized before runBehavior is called");
return;
}

if(global_costmap_ == NULL || local_costmap_ == NULL){
ROS_ERROR("The costmaps passed to the RotateRecovery object cannot be NULL. Doing nothing.");
if (local_costmap_ == NULL)
{
ROS_ERROR("The costmap passed to the RotateRecovery object cannot be NULL. Doing nothing.");
return;
}
ROS_WARN("Rotate recovery behavior started.");
Expand All @@ -100,41 +112,62 @@ void RotateRecovery::runBehavior(){
geometry_msgs::PoseStamped global_pose;
local_costmap_->getRobotPose(global_pose);

double current_angle = -1.0 * M_PI;
double current_angle = tf2::getYaw(global_pose.pose.orientation));
double start_angle = current_angle;

bool got_180 = false;

double start_offset = 0 - angles::normalize_angle(tf2::getYaw(global_pose.pose.orientation));
while(n.ok()){
while (n.ok() &&
(!got_180 ||
std::fabs(angles::shortest_angular_distance(current_angle, start_angle)) > tolerance_))
{
// Update Current Angle
local_costmap_->getRobotPose(global_pose);

double norm_angle = angles::normalize_angle(tf2::getYaw(global_pose.pose.orientation));
current_angle = angles::normalize_angle(norm_angle + start_offset);

//compute the distance left to rotate
double dist_left = M_PI - current_angle;
current_angle = tf2::getYaw(global_pose.pose.orientation));

// compute the distance left to rotate
double dist_left;
if (!got_180)
{
// If we haven't hit 180 yet, we need to rotate a half circle plus the distance to the 180 point
double distance_to_180 = std::fabs(angles::shortest_angular_distance(current_angle, start_angle + M_PI));
dist_left = M_PI + distance_to_180;

if (distance_to_180 < tolerance_)
{
got_180 = true;
}
}
else
{
// If we have hit the 180, we just have the distance back to the start
dist_left = std::fabs(angles::shortest_angular_distance(current_angle, start_angle));
}

double x = global_pose.pose.position.x, y = global_pose.pose.position.y;

//check if that velocity is legal by forward simulating
// check if that velocity is legal by forward simulating
double sim_angle = 0.0;
while(sim_angle < dist_left){
double theta = tf2::getYaw(global_pose.pose.orientation) + sim_angle;
while (sim_angle < dist_left)
{
double theta = current_angle + sim_angle;

//make sure that the point is legal, if it isn't... we'll abort
// make sure that the point is legal, if it isn't... we'll abort
double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
if(footprint_cost < 0.0){
ROS_ERROR("Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f", footprint_cost);
if (footprint_cost < 0.0)
{
ROS_ERROR("Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f",
footprint_cost);
return;
}

sim_angle += sim_granularity_;
}

//compute the velocity that will let us stop by the time we reach the goal
// compute the velocity that will let us stop by the time we reach the goal
double vel = sqrt(2 * acc_lim_th_ * dist_left);

//make sure that this velocity falls within the specified limits
// make sure that this velocity falls within the specified limits
vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);

geometry_msgs::Twist cmd_vel;
Expand All @@ -144,15 +177,7 @@ void RotateRecovery::runBehavior(){

vel_pub.publish(cmd_vel);

//makes sure that we won't decide we're done right after we start
if(current_angle < 0.0)
got_180 = true;

//if we're done with our in-place rotation... then return
if(got_180 && current_angle >= (0.0 - tolerance_))
return;

r.sleep();
}
}
};
}; // namespace rotate_recovery

0 comments on commit b011ca0

Please sign in to comment.