diff --git a/rotate_recovery/include/rotate_recovery/rotate_recovery.h b/rotate_recovery/include/rotate_recovery/rotate_recovery.h index 1a455b6624..cbfdf2daee 100644 --- a/rotate_recovery/include/rotate_recovery/rotate_recovery.h +++ b/rotate_recovery/include/rotate_recovery/rotate_recovery.h @@ -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 #include #include -#include #include -#include -#include -#include +#include -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 diff --git a/rotate_recovery/src/rotate_recovery.cpp b/rotate_recovery/src/rotate_recovery.cpp index e8539a63dc..152babff43 100644 --- a/rotate_recovery/src/rotate_recovery.cpp +++ b/rotate_recovery/src/rotate_recovery.cpp @@ -38,28 +38,35 @@ #include #include #include +#include +#include +#include +#include +#include +#include -//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); @@ -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."); @@ -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; @@ -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