latched_stop_rotate_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * latched_stop_rotate_controller.cpp
00003  *
00004  *  Created on: Apr 16, 2012
00005  *      Author: tkruse
00006  */
00007 
00008 #include <base_local_planner/latched_stop_rotate_controller.h>
00009 
00010 #include <cmath>
00011 
00012 #include <Eigen/Core>
00013 
00014 #include <angles/angles.h>
00015 #include <nav_msgs/Odometry.h>
00016 
00017 #include <base_local_planner/goal_functions.h>
00018 #include <base_local_planner/local_planner_limits.h>
00019 
00020 namespace base_local_planner {
00021 
00022 LatchedStopRotateController::LatchedStopRotateController(const std::string& name) {
00023   ros::NodeHandle private_nh("~/" + name);
00024   private_nh.param("latch_xy_goal_tolerance", latch_xy_goal_tolerance_, false);
00025 
00026   rotating_to_goal_ = false;
00027 }
00028 
00029 LatchedStopRotateController::~LatchedStopRotateController() {}
00030 
00031 
00037 bool LatchedStopRotateController::isPositionReached(LocalPlannerUtil* planner_util,
00038     tf::Stamped<tf::Pose> global_pose) {
00039   double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
00040 
00041   //we assume the global goal is the last point in the global plan
00042   tf::Stamped<tf::Pose> goal_pose;
00043   if ( ! planner_util->getGoal(goal_pose)) {
00044     return false;
00045   }
00046 
00047   double goal_x = goal_pose.getOrigin().getX();
00048   double goal_y = goal_pose.getOrigin().getY();
00049 
00050   //check to see if we've reached the goal position
00051   if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
00052       base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
00053     xy_tolerance_latch_ = true;
00054     return true;
00055   }
00056   return false;
00057 }
00058 
00059 
00064 bool LatchedStopRotateController::isGoalReached(LocalPlannerUtil* planner_util,
00065     OdometryHelperRos& odom_helper,
00066     tf::Stamped<tf::Pose> global_pose) {
00067   double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
00068   double rot_stopped_vel = planner_util->getCurrentLimits().rot_stopped_vel;
00069   double trans_stopped_vel = planner_util->getCurrentLimits().trans_stopped_vel;
00070 
00071   //copy over the odometry information
00072   nav_msgs::Odometry base_odom;
00073   odom_helper.getOdom(base_odom);
00074 
00075   //we assume the global goal is the last point in the global plan
00076   tf::Stamped<tf::Pose> goal_pose;
00077   if ( ! planner_util->getGoal(goal_pose)) {
00078     return false;
00079   }
00080 
00081   double goal_x = goal_pose.getOrigin().getX();
00082   double goal_y = goal_pose.getOrigin().getY();
00083 
00084   base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
00085 
00086   //check to see if we've reached the goal position
00087   if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
00088       base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
00089     //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
00090     //just rotate in place
00091     if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_) {
00092       ROS_DEBUG("Goal position reached (check), stopping and turning in place");
00093       xy_tolerance_latch_ = true;
00094     }
00095     double goal_th = tf::getYaw(goal_pose.getRotation());
00096     double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
00097     //check to see if the goal orientation has been reached
00098     if (fabs(angle) <= limits.yaw_goal_tolerance) {
00099       //make sure that we're actually stopped before returning success
00100       if (base_local_planner::stopped(base_odom, rot_stopped_vel, trans_stopped_vel)) {
00101         return true;
00102       }
00103     }
00104   }
00105   return false;
00106 }
00107 
00108 bool LatchedStopRotateController::stopWithAccLimits(const tf::Stamped<tf::Pose>& global_pose,
00109     const tf::Stamped<tf::Pose>& robot_vel,
00110     geometry_msgs::Twist& cmd_vel,
00111     Eigen::Vector3f acc_lim,
00112     double sim_period,
00113     boost::function<bool (Eigen::Vector3f pos,
00114                           Eigen::Vector3f vel,
00115                           Eigen::Vector3f vel_samples)> obstacle_check) {
00116 
00117   //slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible
00118   //but we'll use a tenth of a second to be consistent with the implementation of the local planner.
00119   double vx = sign(robot_vel.getOrigin().x()) * std::max(0.0, (fabs(robot_vel.getOrigin().x()) - acc_lim[0] * sim_period));
00120   double vy = sign(robot_vel.getOrigin().y()) * std::max(0.0, (fabs(robot_vel.getOrigin().y()) - acc_lim[1] * sim_period));
00121 
00122   double vel_yaw = tf::getYaw(robot_vel.getRotation());
00123   double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim[2] * sim_period));
00124 
00125   //we do want to check whether or not the command is valid
00126   double yaw = tf::getYaw(global_pose.getRotation());
00127   bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw),
00128                                   Eigen::Vector3f(robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), vel_yaw),
00129                                   Eigen::Vector3f(vx, vy, vth));
00130 
00131   //if we have a valid command, we'll pass it on, otherwise we'll command all zeros
00132   if(valid_cmd){
00133     ROS_DEBUG_NAMED("latched_stop_rotate", "Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
00134     cmd_vel.linear.x = vx;
00135     cmd_vel.linear.y = vy;
00136     cmd_vel.angular.z = vth;
00137     return true;
00138   }
00139   ROS_WARN("Stopping cmd in collision");
00140   cmd_vel.linear.x = 0.0;
00141   cmd_vel.linear.y = 0.0;
00142   cmd_vel.angular.z = 0.0;
00143   return false;
00144 }
00145 
00146 bool LatchedStopRotateController::rotateToGoal(
00147     const tf::Stamped<tf::Pose>& global_pose,
00148     const tf::Stamped<tf::Pose>& robot_vel,
00149     double goal_th,
00150     geometry_msgs::Twist& cmd_vel,
00151     Eigen::Vector3f acc_lim,
00152     double sim_period,
00153     base_local_planner::LocalPlannerLimits& limits,
00154     boost::function<bool (Eigen::Vector3f pos,
00155                           Eigen::Vector3f vel,
00156                           Eigen::Vector3f vel_samples)> obstacle_check) {
00157   double yaw = tf::getYaw(global_pose.getRotation());
00158   double vel_yaw = tf::getYaw(robot_vel.getRotation());
00159   cmd_vel.linear.x = 0;
00160   cmd_vel.linear.y = 0;
00161   double ang_diff = angles::shortest_angular_distance(yaw, goal_th);
00162 
00163   double v_theta_samp = std::min(limits.max_rot_vel, std::max(limits.min_rot_vel, fabs(ang_diff)));
00164 
00165   //take the acceleration limits of the robot into account
00166   double max_acc_vel = fabs(vel_yaw) + acc_lim[2] * sim_period;
00167   double min_acc_vel = fabs(vel_yaw) - acc_lim[2] * sim_period;
00168 
00169   v_theta_samp = std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);
00170 
00171   //we also want to make sure to send a velocity that allows us to stop when we reach the goal given our acceleration limits
00172   double max_speed_to_stop = sqrt(2 * acc_lim[2] * fabs(ang_diff));
00173   v_theta_samp = std::min(max_speed_to_stop, fabs(v_theta_samp));
00174 
00175   v_theta_samp = std::min(limits.max_rot_vel, std::max(limits.min_rot_vel, v_theta_samp));
00176 
00177   if (ang_diff < 0) {
00178     v_theta_samp = - v_theta_samp;
00179   }
00180 
00181   //we still want to lay down the footprint of the robot and check if the action is legal
00182   bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw),
00183       Eigen::Vector3f(robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), vel_yaw),
00184       Eigen::Vector3f( 0.0, 0.0, v_theta_samp));
00185 
00186   if (valid_cmd) {
00187     ROS_DEBUG_NAMED("dwa_local_planner", "Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
00188     cmd_vel.angular.z = v_theta_samp;
00189     return true;
00190   }
00191   ROS_WARN("Rotation cmd in collision");
00192   cmd_vel.angular.z = 0.0;
00193   return false;
00194 
00195 }
00196 
00197 bool LatchedStopRotateController::computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
00198     Eigen::Vector3f acc_lim,
00199     double sim_period,
00200     LocalPlannerUtil* planner_util,
00201     OdometryHelperRos& odom_helper_,
00202     tf::Stamped<tf::Pose> global_pose,
00203     boost::function<bool (Eigen::Vector3f pos,
00204                           Eigen::Vector3f vel,
00205                           Eigen::Vector3f vel_samples)> obstacle_check) {
00206   //we assume the global goal is the last point in the global plan
00207   tf::Stamped<tf::Pose> goal_pose;
00208   if ( ! planner_util->getGoal(goal_pose)) {
00209     ROS_ERROR("Could not get goal pose");
00210     return false;
00211   }
00212 
00213   base_local_planner::LocalPlannerLimits limits = planner_util->getCurrentLimits();
00214 
00215   //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
00216   //just rotate in place
00217   if (latch_xy_goal_tolerance_ && ! xy_tolerance_latch_ ) {
00218     ROS_INFO("Goal position reached, stopping and turning in place");
00219     xy_tolerance_latch_ = true;
00220   }
00221   //check to see if the goal orientation has been reached
00222   double goal_th = tf::getYaw(goal_pose.getRotation());
00223   double angle = base_local_planner::getGoalOrientationAngleDifference(global_pose, goal_th);
00224   if (fabs(angle) <= limits.yaw_goal_tolerance) {
00225     //set the velocity command to zero
00226     cmd_vel.linear.x = 0.0;
00227     cmd_vel.linear.y = 0.0;
00228     cmd_vel.angular.z = 0.0;
00229     rotating_to_goal_ = false;
00230   } else {
00231     ROS_DEBUG("Angle: %f Tolerance: %f", angle, limits.yaw_goal_tolerance);
00232     tf::Stamped<tf::Pose> robot_vel;
00233     odom_helper_.getRobotVel(robot_vel);
00234     nav_msgs::Odometry base_odom;
00235     odom_helper_.getOdom(base_odom);
00236 
00237     //if we're not stopped yet... we want to stop... taking into account the acceleration limits of the robot
00238     if ( ! rotating_to_goal_ && !base_local_planner::stopped(base_odom, limits.rot_stopped_vel, limits.trans_stopped_vel)) {
00239       if ( ! stopWithAccLimits(
00240           global_pose,
00241           robot_vel,
00242           cmd_vel,
00243           acc_lim,
00244           sim_period,
00245           obstacle_check)) {
00246         ROS_INFO("Error when stopping.");
00247         return false;
00248       }
00249       ROS_DEBUG("Stopping...");
00250     }
00251     //if we're stopped... then we want to rotate to goal
00252     else {
00253       //set this so that we know its OK to be moving
00254       rotating_to_goal_ = true;
00255       if ( ! rotateToGoal(
00256           global_pose,
00257           robot_vel,
00258           goal_th,
00259           cmd_vel,
00260           acc_lim,
00261           sim_period,
00262           limits,
00263           obstacle_check)) {
00264         ROS_INFO("Error when rotating.");
00265         return false;
00266       }
00267       ROS_DEBUG("Rotating...");
00268     }
00269   }
00270 
00271   return true;
00272 
00273 }
00274 
00275 
00276 } /* namespace base_local_planner */


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:38