diff --git a/autonomous_robotics_ros/src/navigation/rotate_recovery/include/rotate_recovery/rotate_recovery.h b/autonomous_robotics_ros/src/navigation/rotate_recovery/include/rotate_recovery/rotate_recovery.h index 1aaf425..e06c0e6 100644 --- a/autonomous_robotics_ros/src/navigation/rotate_recovery/include/rotate_recovery/rotate_recovery.h +++ b/autonomous_robotics_ros/src/navigation/rotate_recovery/include/rotate_recovery/rotate_recovery.h @@ -77,6 +77,7 @@ public: ~RotateRecovery(); private: + costmap_2d::Costmap2DROS* global_costmap_; costmap_2d::Costmap2DROS* local_costmap_; bool initialized_; double sim_granularity_, min_rotational_vel_, max_rotational_vel_, acc_lim_th_, tolerance_, frequency_; diff --git a/autonomous_robotics_ros/src/navigation/rotate_recovery/src/rotate_recovery.cpp b/autonomous_robotics_ros/src/navigation/rotate_recovery/src/rotate_recovery.cpp index 65959e8..c1ae6db 100755 --- a/autonomous_robotics_ros/src/navigation/rotate_recovery/src/rotate_recovery.cpp +++ b/autonomous_robotics_ros/src/navigation/rotate_recovery/src/rotate_recovery.cpp @@ -3,7 +3,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2009, Willow Garage, Inc. -* Copyright (c) 2017, Texas Instruments Incorporated * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,107 +36,142 @@ *********************************************************************/ #include #include +#include +#include +#include +#include +#include +#include +#include +#include -//register this planner as a RecoveryBehavior plugin + +// register this planner as a RecoveryBehavior plugin PLUGINLIB_EXPORT_CLASS(rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior) -namespace rotate_recovery { -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, tf::TransformListener* 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* global_costmap, costmap_2d::Costmap2DROS* local_costmap) +{ + if (!initialized_) + { local_costmap_ = local_costmap; + global_costmap_ = global_costmap; + // get some parameters from the parameter server + ros::NodeHandle private_nh("~/" + name); + ros::NodeHandle blp_nh("~/TrajectoryPlannerROS"); - //get some parameters from the parameter server - ros::NodeHandle private_nh("~/" + name_); - ros::NodeHandle blp_nh("~/DWAPlannerROS"); - - //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); - blp_nh.param("acc_lim_th", acc_lim_th_, 3.2); - blp_nh.param("max_rotational_vel", max_rotational_vel_, 1.0); - blp_nh.param("min_in_place_rotational_vel", min_rotational_vel_, 0.4); + acc_lim_th_ = nav_core::loadParameterWithDeprecation(blp_nh, "acc_lim_theta", "acc_lim_th", 3.2); + max_rotational_vel_ = nav_core::loadParameterWithDeprecation(blp_nh, "max_vel_theta", "max_rotational_vel", 1.0); + min_rotational_vel_ = nav_core::loadParameterWithDeprecation(blp_nh, "min_in_place_vel_theta", "min_in_place_rotational_vel", 0.4); blp_nh.param("yaw_goal_tolerance", tolerance_, 0.10); world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap()); 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("mmWave customized rotate recovery behavior started."); + ROS_WARN("Rotate recovery behavior started."); ROS_WARN("Clearing costmaps..."); global_costmap_->resetLayers(); local_costmap_->resetLayers(); - ROS_WARN("Performing rotation..."); ros::Rate r(frequency_); ros::NodeHandle n; ros::Publisher vel_pub = n.advertise("cmd_vel", 10); - tf::Stamped global_pose; + 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 = angles::normalize_angle_positive(tf::getYaw(global_pose.getRotation())); - 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); + current_angle = tf2::getYaw(global_pose.pose.orientation); - double norm_angle = angles::normalize_angle_positive(tf::getYaw(global_pose.getRotation())); - current_angle = angles::normalize_angle_positive(norm_angle - start_offset); + // 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; - //compute the distance left to rotate - double dist_left = (2 * M_PI) - current_angle; + 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.getOrigin().x(), y = global_pose.getOrigin().y(); + 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 = tf::getYaw(global_pose.getRotation()) + 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; @@ -147,23 +181,7 @@ void RotateRecovery::runBehavior(){ vel_pub.publish(cmd_vel); - //makes sure that we won't decide we're done right after we start - if ((fabs(current_angle - M_PI) < M_PI/2) && (got_180 == false)) - { - got_180 = true; - } - - //if we're done with our in-place rotation... then return - if(got_180 && ((current_angle >= (2*M_PI - tolerance_)) || (current_angle <= (M_PI/2)))) - { - cmd_vel.angular.z = 0; - vel_pub.publish(cmd_vel); - - ROS_WARN("Completed rotation..."); - return; - } - r.sleep(); } } -}; +}; // namespace rotate_recovery