00001
00002
00003
00004
00005
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
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
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
00072 nav_msgs::Odometry base_odom;
00073 odom_helper.getOdom(base_odom);
00074
00075
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
00087 if ((latch_xy_goal_tolerance_ && xy_tolerance_latch_) ||
00088 base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) {
00089
00090
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
00098 if (fabs(angle) <= limits.yaw_goal_tolerance) {
00099
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
00118
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
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
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
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
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
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
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
00216
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
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
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
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
00252 else {
00253
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 }