00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00042 #include "cart_local_planner/cart_local_planner.h"
00043 #include <base_local_planner/goal_functions.h>
00044 #include <tf_conversions/tf_kdl.h>
00045 #include <pluginlib/class_list_macros.h>
00046 #include <vector>
00047 #include <boost/foreach.hpp>
00048 #include <boost/optional.hpp>
00049
00050 PLUGINLIB_DECLARE_CLASS(cart_local_planner, CartLocalPlanner, cart_local_planner::CartLocalPlanner, nav_core::BaseLocalPlanner)
00051
00052 namespace gm = geometry_msgs;
00053
00054
00055 namespace cart_pushing_msgs {
00056
00057
00058 bool operator==(const cart_pushing_msgs::RobotCartPath& sbpl_plan,
00059 const std::vector<gm::PoseStamped>& mb_plan)
00060
00061 {
00062 const unsigned n = sbpl_plan.path.size();
00063 if (n != mb_plan.size())
00064 {
00065 ROS_ERROR("Plans don't have the same size: %d, SBPL: %d",(int)mb_plan.size(),(int)sbpl_plan.path.size());
00066 return false;
00067 }
00068 for (unsigned i = 0; i < n; i++) {
00069
00070 const gm::Pose& p = mb_plan[i].pose;
00071 const gm::Pose& p2 = sbpl_plan.path[i].robot_pose;
00072 if ((p.position.x != p2.position.x) || (p.position.y != p2.position.y)
00073 || (p.position.z != p2.position.z) || (p.orientation.x
00074 != p2.orientation.x) || (p.orientation.y != p2.orientation.y)
00075 || (p.orientation.z != p2.orientation.z) || (p.orientation.w
00076 != p2.orientation.w))
00077 {
00078 ROS_ERROR("Plans don't have the same waypoint: %d",i);
00079 return false;
00080 }
00081 }
00082 return true;
00083 }
00084 }
00085
00086 namespace cart_local_planner {
00087 using std::vector;
00088 using std::max;
00089 using std::string;
00090 using boost::optional;
00091 const double SBPL_DTHRESH=-0.013;
00092 typedef tf::Stamped<tf::Pose> StampedPose;
00093
00094 template<class T>
00095 T clamp(T val, T abs_max) {
00096 return val > 0 ? (val > abs_max ? abs_max : val)
00097 : (val < -abs_max ? -abs_max : val);
00098 }
00099
00100 const gm::Twist operator+(const gm::Twist& t1, const gm::Twist& t2)
00101 {
00102 gm::Twist res;
00103 res.linear.x = t1.linear.x + t2.linear.x;
00104 res.linear.y = t1.linear.y + t2.linear.y;
00105 res.linear.z = t1.linear.z + t2.linear.z;
00106 res.angular.x = t1.angular.x + t2.angular.x;
00107 res.angular.y = t1.angular.y + t2.angular.y;
00108 res.angular.z = t1.angular.z + t2.angular.z;
00109
00110 return res;
00111 }
00112
00113 const gm::Twist operator-(const gm::Twist& t1, const gm::Twist& t2)
00114 {
00115 gm::Twist res;
00116 res.linear.x = t1.linear.x - t2.linear.x;
00117 res.linear.y = t1.linear.y - t2.linear.y;
00118 res.linear.z = t1.linear.z - t2.linear.z;
00119 res.angular.x = t1.angular.x - t2.angular.x;
00120 res.angular.y = t1.angular.y - t2.angular.y;
00121 res.angular.z = t1.angular.z - t2.angular.z;
00122
00123 return res;
00124 }
00125
00126 double mag(gm::Twist &t) {
00127 return sqrt(t.linear.x*t.linear.x + t.linear.y*t.linear.y + t.linear.z*t.linear.z +
00128 t.angular.x*t.angular.x + t.angular.y*t.angular.y + t.angular.z*t.angular.z);
00129 }
00130
00131 double mag(geometry_msgs::Pose2D &p) {
00132 return sqrt(p.x*p.x+p.y*p.y+p.theta*p.theta);
00133 }
00134
00135 CartLocalPlanner::CartLocalPlanner() :
00136 tf_(NULL), nh_("cart_planner_costmap"), costmap_ros_(NULL), initialized_(false)
00137 {
00138 }
00139
00140 template <typename T>
00141 void getParam (const ros::NodeHandle nh, const string& name, T* place)
00142 {
00143 bool found = nh.getParam(name, *place);
00144 ROS_ASSERT_MSG (found, "Did not find parameter %s", nh.resolveName(name).c_str());
00145 }
00146
00147 void CartLocalPlanner::initialize(std::string name, tf::TransformListener* tf,
00148 costmap_2d::Costmap2DROS* costmap_ros) {
00149 tf_ = tf;
00150 costmap_ros_ = costmap_ros;
00151 current_waypoint_ = 0;
00152 goal_reached_time_ = ros::Time::now();
00153 ros::NodeHandle cart_nh("cart_pushing");
00154 ros::NodeHandle nh("~/"+name);
00155
00156
00157 std::string cart_param_ns;
00158 getParam(nh, "cart_param_namespace", &cart_param_ns);
00159 ros::NodeHandle cart_param_nh(cart_param_ns);
00160
00161
00162 robot_collision_checker_.initialize(costmap_ros_, "robot_trajectory");
00163
00164
00165 getParam(cart_nh, "max_vel_base_x", &twist_base_max_.linear.x);
00166 getParam(cart_nh, "max_vel_base_y", &twist_base_max_.linear.y);
00167 getParam(cart_nh, "max_vel_base_theta", &twist_base_max_.angular.z);
00168 getParam(cart_nh, "max_vel_cart_x", &twist_cart_max_.linear.x);
00169 getParam(cart_nh, "max_vel_cart_y", &twist_cart_max_.linear.y);
00170 getParam(cart_nh, "max_vel_cart_theta", &twist_cart_max_.angular.z);
00171
00172
00173 getParam(nh, "dt", &dt_);
00174 getParam(nh, "num_traj_steps", &num_traj_steps_);
00175
00176
00177 double cart_length, cart_width, cart_x_offset, cart_y_offset;
00178 getParam(cart_param_nh, "length", &cart_length);
00179 getParam(cart_param_nh, "width", &cart_width);
00180 getParam(cart_param_nh, "footprint_x_offset", &cart_x_offset);
00181 getParam(cart_param_nh, "footprint_y_offset", &cart_y_offset);
00182 cart_collision_checker_.initialize(costmap_ros_, "cart_trajectory");
00183 cart_collision_checker_.setFootprint(cart_length, cart_width, cart_x_offset, cart_y_offset);
00184 cart_collision_checker_.setRobotFrameID("cart");
00185
00186 getParam(nh, "k_trans_base", &K_trans_base_);
00187 getParam(nh, "k_rot_base", &K_rot_base_);
00188 getParam(nh, "k_trans_cart", &K_trans_cart_);
00189 getParam(nh, "k_rot_cart", &K_rot_cart_);
00190
00191 getParam(nh, "subscribe_sbpl_plan", &subscribe_sbpl_plan_);
00192
00193 ros::NodeHandle node;
00194 odom_sub_ = node.subscribe<nav_msgs::Odometry> ("odom", 1, &CartLocalPlanner::odomCallback, this);
00195 invalid_pose_sub_ = node.subscribe<std_msgs::Empty> ("cart_pushing/articulate_cart_server/invalid_pose",
00196 1, &CartLocalPlanner::invalidPoseCallback, this);
00197 vel_pub_ = node.advertise<gm::Twist> ("base_controller/command", 10);
00198
00199 if(subscribe_sbpl_plan_)
00200 {
00201 ROS_INFO("Setting up SBPL subscriber");
00202 sbpl_subscriber_.reset(new cart_local_planner::SBPLSubscriber<cart_pushing_msgs::RobotCartPath>(node,"/move_base_node/SBPLCartPlanner/sbpl_robot_cart_plan"));
00203 }
00204
00205
00206 std::string cart_articulation_ns;
00207 getParam(nh, "cart_articulation_namespace", &cart_articulation_ns);
00208 cart_pose_pub_ = node.advertise<gm::PoseStamped>(cart_articulation_ns + "/command_pose", 10);
00209 cart_twist_pub_ = node.advertise<gm::TwistStamped>(cart_articulation_ns + "/command_twist", 10);
00210
00211
00212 getParam(cart_param_nh, "x_min", &cart_range.x_min);
00213 getParam(cart_param_nh, "x_max", &cart_range.x_max);
00214 getParam(cart_param_nh, "y_min", &cart_range.y_min);
00215 getParam(cart_param_nh, "y_max", &cart_range.y_max);
00216 getParam(cart_param_nh, "t_min", &cart_range.t_min);
00217 getParam(cart_param_nh, "t_max", &cart_range.t_max);
00218 ROS_DEBUG("Loaded workspace range from %s: %.2lf %.2lf, %.2lf %.2lf, %.2lf %.2lf", cart_param_nh.getNamespace().c_str(), cart_range.x_min, cart_range.x_max,
00219 cart_range.y_min, cart_range.y_max, cart_range.t_min, cart_range.t_max);
00220
00221
00222 getParam(nh, "tolerance_trans", &tolerance_trans_);
00223 getParam(nh, "tolerance_rot", &tolerance_rot_);
00224 getParam(nh, "tolerance_timeout", &tolerance_timeout_);
00225 getParam(nh, "trans_stopped_velocity", &trans_stopped_velocity_);
00226 getParam(nh, "rot_stopped_velocity", &rot_stopped_velocity_);
00227
00228
00229 initialization_extras();
00230
00231
00232 pose2D_pub_ = nh.advertise<gm::Pose2D> ("debug_pose", 10);
00233
00234 initialized_ = true;
00235 ROS_DEBUG("Initialized");
00236 }
00237
00238 void CartLocalPlanner::initialization_extras()
00239 {
00240 }
00241
00242 bool CartLocalPlanner::computeVelocityCommands(gm::Twist& cmd_vel)
00243 {
00244 {
00245 static unsigned debug_counter=0;
00246 debug_counter++;
00247 debug_print_ = !(debug_counter%20);
00248 }
00249
00251 tf::Stamped<tf::Pose> robot_pose;
00252 static ros::Time last_good_command;
00253
00254 ROS_DEBUG_COND_NAMED (debug_print_, "loop", "Computing velocity commands");
00255
00256
00257
00258 if (!costmap_ros_->getRobotPose(robot_pose)) {
00259 freeze();
00260 ros::Duration time_since_last_good_command = ros::Time::now()-last_good_command;
00261 ROS_WARN_STREAM("Can't get robot pose; time since last good command is " << time_since_last_good_command);
00262 return time_since_last_good_command < ros::Duration(2.0);
00263 }
00265 tf::StampedTransform cart_pose;
00266 tf_->lookupTransform("base_footprint", "cart", ros::Time(), cart_pose);
00267
00269 robot_pose_actual_.setBasis(robot_pose.getBasis());
00270 robot_pose_actual_.setOrigin(robot_pose.getOrigin());
00271 cart_pose_actual_.setBasis(cart_pose.getBasis());
00272 cart_pose_actual_.setOrigin(cart_pose.getOrigin());
00273
00275 twist_base_ = &cmd_vel;
00276
00278 setGoalPoses();
00279
00280 ROS_DEBUG_COND_NAMED (debug_print_, "loop", " Set goal poses");
00281
00283 setControlMode();
00284
00286 controlModeAction();
00287
00288 ROS_DEBUG_COND_NAMED (debug_print_, "loop", " Generated controls");
00289
00290
00291 bool found_good_vels = false;
00292
00293
00294
00296 found_good_vels = checkTwists();
00297 if (found_good_vels)
00298 {
00299 last_good_command = ros::Time::now();
00300 twist_cart_.twist.linear.z = 0.0;
00301 twist_cart_.twist.angular.x = 0.0;
00302 twist_cart_.twist.angular.y = 0.0;
00303 cart_twist_pub_.publish(twist_cart_);
00304 }
00305 else
00306 {
00307 found_good_vels = checkTwistsMonotonic();
00308 if (found_good_vels)
00309 {
00310 ROS_INFO("Twists are taking us away from collision");
00311 last_good_command = ros::Time::now();
00312 twist_cart_.twist.linear.z = 0.0;
00313 twist_cart_.twist.angular.x = 0.0;
00314 twist_cart_.twist.angular.y = 0.0;
00315 cart_twist_pub_.publish(twist_cart_);
00316 }
00317 else
00318 {
00319 ROS_WARN_THROTTLE(1, "Freezing because the twists are leading to collision");
00320 freeze();
00321 }
00322 }
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344 publishDebugPose(robot_pose_actual_);
00345 ROS_DEBUG_COND_NAMED(debug_print_, "loop", "final base twist: [%.3f,%.3f][%.3f]",
00346 twist_base_->linear.x, twist_base_->linear.y, twist_base_->angular.z);
00347 ROS_DEBUG_COND_NAMED(debug_print_, "loop", "final cart twist: [%.3f,%.3f][%.3f]",
00348 twist_cart_.twist.linear.x, twist_cart_.twist.linear.y, twist_cart_.twist.angular.z);
00349
00350 return found_good_vels;
00351 }
00352
00353 bool CartLocalPlanner::checkTrajectoryMonotonic(const std::vector<unsigned int> &indices)
00354 {
00355 boost::optional<cart_pushing_msgs::RobotCartPath> sbpl_plan = sbpl_subscriber_->lookupPlan(original_global_plan_);
00356 if(!sbpl_plan)
00357 {
00358 ROS_ERROR("Could not find SBPL plan");
00359 return false;
00360 }
00361 std::vector<geometry_msgs::Pose2D> robot_path, cart_path;
00362
00363 for(unsigned int i=0; i <indices.size(); i++)
00364 {
00365 unsigned int index = indices[i];
00366
00367 geometry_msgs::Pose2D robot_pose_2d;
00368 robot_pose_2d.x = global_plan_[index].pose.position.x;
00369 robot_pose_2d.y = global_plan_[index].pose.position.y;
00370 robot_pose_2d.theta = tf::getYaw(global_plan_[index].pose.orientation);
00371 robot_path.push_back(robot_pose_2d);
00372
00373 tf::Pose robot_pose, cart_pose_local, cart_pose_global;
00374 tf::poseMsgToTF(global_plan_[index].pose,robot_pose);
00375 tf::poseMsgToTF(sbpl_plan->path[index].cart_pose,cart_pose_local);
00376 cart_pose_global = robot_pose*cart_pose_local;
00377
00378 geometry_msgs::Pose2D cart_pose;
00379 cart_pose.x = cart_pose_global.getOrigin().x();
00380 cart_pose.y = cart_pose_global.getOrigin().y();
00381 cart_pose.theta = tf::getYaw(cart_pose_global.getRotation());
00382 cart_path.push_back(cart_pose);
00383
00384 }
00385
00386
00387 double base_footprint_cost=robot_collision_checker_.checkTrajectoryMonotonic(robot_path,true,true,5);
00388 if(base_footprint_cost < 0.0)
00389 {
00390 ROS_INFO("Base footprint in collision");
00391 return false;
00392 }
00393
00394 double cart_footprint_cost=cart_collision_checker_.checkTrajectoryMonotonic(cart_path,true,true,5);
00395 if(cart_footprint_cost < 0.0)
00396 {
00397 ROS_INFO("Cart footprint in collision");
00398 return false;
00399 }
00400 return true;
00401 }
00402
00403 bool CartLocalPlanner::getNextFewWaypointsIndices(const std::vector<geometry_msgs::PoseStamped> ¤t_plan,
00404 const int ¤t_waypoint_index,
00405 const int &max_num_waypoints,
00406 const double &max_translation,
00407 const double &max_rotation,
00408 std::vector<unsigned int> &waypoint_indices)
00409 {
00410 if(current_waypoint_index > (int)current_plan.size()-1)
00411 {
00412 ROS_ERROR("Current waypoint exceeds number of points in plan");
00413 return false;
00414 }
00415 unsigned int num_waypoints = 0;
00416 waypoint_indices.clear();
00417
00418 tf::Stamped<tf::Pose> current_waypoint;
00419 tf::poseStampedMsgToTF(current_plan[current_waypoint_index],current_waypoint);
00420 for(unsigned int i = current_waypoint_index; i < current_plan.size(); i++)
00421 {
00422 tf::Stamped<tf::Pose> next_waypoint;
00423 tf::poseStampedMsgToTF(current_plan[i],next_waypoint);
00424 tf::Pose error = current_waypoint.inverseTimes(next_waypoint);
00425 if(error.getOrigin().length() > max_translation)
00426 break;
00427 if(fabs(tf::getYaw(error.getRotation())) > max_rotation)
00428 break;
00429 waypoint_indices.push_back(i);
00430 num_waypoints++;
00431 if((int)num_waypoints > max_num_waypoints)
00432 break;
00433 }
00434 return true;
00435 }
00436
00437 void CartLocalPlanner::setRobotPoseGoal (const StampedPose& goal)
00438 {
00439 robot_pose_goal_ = goal;
00440 robot_pose_error_ = robot_pose_actual_.inverseTimes(robot_pose_goal_);
00441 }
00442
00443 void CartLocalPlanner::setCartPoseGoal (const StampedPose& goal)
00444 {
00445 cart_pose_goal_ = goal;
00446 cart_pose_error_.x = cart_pose_goal_.getOrigin().x() - cart_pose_actual_.getOrigin().x();
00447 cart_pose_error_.y = cart_pose_goal_.getOrigin().y() - cart_pose_actual_.getOrigin().y();
00448 const double dtheta = tf::getYaw(cart_pose_goal_.getRotation()) - tf::getYaw(cart_pose_actual_.getRotation());
00449 cart_pose_error_.theta = angles::normalize_angle(dtheta);
00450 ROS_DEBUG("Cart pose error: %f %f %f",cart_pose_error_.x,cart_pose_error_.y,cart_pose_error_.theta);
00451 }
00452
00453 void CartLocalPlanner::setControlMode()
00454 {
00455 control_mode_ = REGULAR;
00456 }
00457
00458 void CartLocalPlanner::controlModeAction()
00459 {
00460 switch (control_mode_) {
00461 case REGULAR:
00462 {
00463
00464 baseTwistFromError();
00465
00466
00467 cartTwistFromError();
00468
00469
00470 filterTwistsCombined(GLOBAL_SCALING);
00471
00472 if (robot_pose_error_.getOrigin().length() < tolerance_trans_ && current_waypoint_ < global_plan_.size()-1 && mag(cart_pose_error_) < 0.1)
00473 current_waypoint_++;
00474 }
00475 break;
00476 default:
00477 ROS_WARN("Unrecognized control mode requested");
00478 break;
00479 }
00480 }
00481
00482 void CartLocalPlanner::setGoalPoses()
00483 {
00484 ros::Time now(ros::Time::now());
00485 StampedPose robot_pose_goal;
00486 tf::poseStampedMsgToTF(global_plan_[current_waypoint_], robot_pose_goal);
00487 setRobotPoseGoal(robot_pose_goal);
00488 setCartGoalFromWaypoint((cart_range.x_max + cart_range.x_min)/2, cart_range.x_min);
00489
00490 tb_.sendTransform(tf::StampedTransform(robot_pose_goal_, now, costmap_ros_->getGlobalFrameID(),
00491 "base_target_pose"));
00492 tb_.sendTransform(tf::StampedTransform(cart_pose_goal_, now, "base_footprint", "cart_target_unfiltered"));
00493
00494 StampedPose cart_pose_goal = cart_pose_goal_;
00495
00496
00497 if (cart_pose_goal.getOrigin().x() < cart_range.x_min)
00498 cart_pose_goal.getOrigin().setX(cart_range.x_min);
00499 if (cart_pose_goal.getOrigin().x() > cart_range.x_max)
00500 cart_pose_goal.getOrigin().setX(cart_range.x_max);
00501
00502 if (cart_pose_goal.getOrigin().y() < cart_range.y_min)
00503 cart_pose_goal.getOrigin().setY(cart_range.y_min);
00504 if (cart_pose_goal.getOrigin().y() > cart_range.y_max)
00505 cart_pose_goal.getOrigin().setY(cart_range.y_max);
00506
00507 double yaw, pitch, roll;
00508 cart_pose_goal.getBasis().getEulerYPR(yaw, pitch, roll);
00509 if (yaw < cart_range.t_min)
00510 cart_pose_goal.getBasis().setEulerYPR(cart_range.t_min, pitch, roll);
00511 if (yaw > cart_range.t_max)
00512 cart_pose_goal.getBasis().setEulerYPR(cart_range.t_max, pitch, roll);
00513
00514
00515 setCartPoseGoal(cart_pose_goal);
00516
00517 tb_.sendTransform(tf::StampedTransform(cart_pose_goal_, now, "base_footprint", "cart_target_pose"));
00518
00519 }
00520
00521 void CartLocalPlanner::filterTwistsCombined(int filter_options)
00522 {
00524 if (filter_options & GLOBAL_SCALING) {
00525 double xv_scale = fabs(twist_base_->linear.x) / twist_base_max_.linear.x;
00526 double yv_scale = fabs(twist_base_->linear.y) / twist_base_max_.linear.y;
00527 double tv_scale = fabs(twist_base_->angular.z) / twist_base_max_.angular.z;
00528
00529 double base_scaling_factor = std::max(xv_scale, std::max(yv_scale, tv_scale));
00530
00531 double xv_cart_scale = fabs(twist_cart_.twist.linear.x) / twist_cart_max_.linear.x;
00532 double yv_cart_scale = fabs(twist_cart_.twist.linear.y) / twist_cart_max_.linear.y;
00533 double tv_cart_scale = fabs(twist_cart_.twist.angular.z)/ twist_cart_max_.angular.z;
00534
00535 double cart_scaling_factor = std::max(xv_cart_scale, std::max(yv_cart_scale, tv_cart_scale));
00536 double scaling_factor = std::max(base_scaling_factor, cart_scaling_factor);
00537
00538
00539 if (scaling_factor > 1.0) {
00540 double scale_mult = 1.0 / scaling_factor;
00541 scaleTwist2D(*twist_base_, scale_mult);
00542 scaleTwist2D(twist_cart_.twist, scale_mult);
00543 ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "twist_filter",
00544 "Scaling, to keep things in range, cart and base twists by " << scale_mult);
00545 }
00546 }
00547
00549 if (filter_options & CART_ERR_SCALING) {
00550 const double scaling_factor = pow(M_E, -50.0 * pow(mag(twist_cart_.twist), 2));
00551 scaleTwist2D(*twist_base_, scaling_factor);
00552 ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "twist_filter",
00553 "Scaling, based on cart error, base velocity by a factor of " << scaling_factor);
00554 }
00555 }
00556
00557
00559 gm::Twist scaleTwist (const gm::Twist& twist, const double scaling_factor)
00560 {
00561 gm::Twist result;
00562 result.linear.x = twist.linear.x*scaling_factor;
00563 result.linear.y = twist.linear.y*scaling_factor;
00564 result.angular.z = twist.angular.z*scaling_factor;
00565 return result;
00566 }
00567
00568 bool CartLocalPlanner::checkTwists()
00569 {
00570 double base_footprint_cost, cart_footprint_cost;
00571 optional<double> cost_of_valid_twist;
00572
00573 for (double scaling_factor=1.0; scaling_factor > 0.7 && !cost_of_valid_twist; scaling_factor *= 0.9) {
00574 const gm::Twist scaled_base_twist = scaleTwist(*twist_base_, scaling_factor);
00575 const gm::Twist scaled_cart_twist = scaleTwist(twist_cart_.twist, scaling_factor);
00576 const gm::Twist base_twist_at_cart = mapBaseTwistToCart(scaled_base_twist);
00577 const gm::Twist net_twist = base_twist_at_cart + scaled_cart_twist;
00578 ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "check_twists", "Checking twists scaled by " << scaling_factor <<
00579 ": " << base_twist_at_cart << ", " << net_twist);
00580
00581
00582 base_footprint_cost=robot_collision_checker_.checkTwist(scaled_base_twist, num_traj_steps_, dt_, true, true);
00583
00584 cart_footprint_cost=cart_collision_checker_.checkTwist(net_twist, num_traj_steps_, dt_, true, false);
00585
00586 if ((base_footprint_cost >= 0) && (cart_footprint_cost >= 0))
00587 cost_of_valid_twist = max(base_footprint_cost, cart_footprint_cost);
00588 }
00589
00590
00591 if (cost_of_valid_twist) {
00592 ROS_DEBUG_COND_NAMED (debug_print_, "check_twists", "Found collision-free twist with cost %.2f",
00593 *cost_of_valid_twist);
00594 return true;
00595 }
00596 else {
00597 ROS_DEBUG_COND_NAMED (debug_print_, "check_twists", "No valid twist found");
00598 return false;
00599 }
00600 }
00601
00602 bool CartLocalPlanner::checkTwistsMonotonic()
00603 {
00604 double base_footprint_cost, cart_footprint_cost;
00605 optional<double> cost_of_valid_twist;
00606 robot_collision_checker_.clearFootprint(true);
00607 cart_collision_checker_.clearFootprint(false);
00608 gm::Twist scaled_base_twist = scaleTwist(*twist_base_, 1.0);
00609 gm::Twist scaled_cart_twist = scaleTwist(twist_cart_.twist, 1.0);
00610 gm::Twist base_twist_at_cart = mapBaseTwistToCart(scaled_base_twist);
00611 gm::Twist net_twist = base_twist_at_cart + scaled_cart_twist;
00612 for (double scaling_factor=1.0; scaling_factor > 0.7 && !cost_of_valid_twist; scaling_factor *= 0.9) {
00613 scaled_base_twist = scaleTwist(*twist_base_, scaling_factor);
00614 scaled_cart_twist = scaleTwist(twist_cart_.twist, scaling_factor);
00615 base_twist_at_cart = mapBaseTwistToCart(scaled_base_twist);
00616 net_twist = base_twist_at_cart + scaled_cart_twist;
00617 ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "check_twists", "Checking twists scaled by " << scaling_factor <<
00618 ": " << base_twist_at_cart << ", " << net_twist);
00619
00620
00621 base_footprint_cost=robot_collision_checker_.checkTwistMonotonic(scaled_base_twist, num_traj_steps_, dt_, false, false);
00622
00623 cart_footprint_cost=cart_collision_checker_.checkTwistMonotonic(net_twist, num_traj_steps_, dt_, false, false);
00624
00625 if ((base_footprint_cost >= 0) && (cart_footprint_cost >= 0))
00626 cost_of_valid_twist = max(base_footprint_cost, cart_footprint_cost);
00627 }
00628
00629
00630 if (cost_of_valid_twist) {
00631 ROS_DEBUG_COND_NAMED (debug_print_, "check_twists", "Found collision-free twist with cost %.2f",
00632 *cost_of_valid_twist);
00633 twist_cart_.twist = scaled_cart_twist;
00634 *twist_base_ = scaled_base_twist;
00635 return true;
00636 }
00637 else {
00638 ROS_DEBUG_COND_NAMED (debug_print_, "check_twists", "No valid twist found");
00639 return false;
00640 }
00641 }
00642
00643
00644 int CartLocalPlanner::setCartGoalFromWaypoint(double min_dist, double min_x_component)
00645 {
00646 StampedPose goal;
00647 if(subscribe_sbpl_plan_)
00648 {
00649 boost::optional<cart_pushing_msgs::RobotCartPath> sbpl_plan = sbpl_subscriber_->lookupPlan(original_global_plan_);
00650 if(sbpl_plan)
00651 {
00652 btTransform goal_transform;
00653 tf::poseMsgToTF(sbpl_plan->path[current_waypoint_].cart_pose,goal_transform);
00654 ROS_DEBUG("Cart pose: %f %f %f, %f %f %f %f",
00655 sbpl_plan->path[current_waypoint_].cart_pose.position.x,
00656 sbpl_plan->path[current_waypoint_].cart_pose.position.y,
00657 sbpl_plan->path[current_waypoint_].cart_pose.position.z,
00658 sbpl_plan->path[current_waypoint_].cart_pose.orientation.x,
00659 sbpl_plan->path[current_waypoint_].cart_pose.orientation.y,
00660 sbpl_plan->path[current_waypoint_].cart_pose.orientation.z,
00661 sbpl_plan->path[current_waypoint_].cart_pose.orientation.w);
00662 goal.setRotation(goal_transform.getRotation());
00663 goal.setOrigin(goal_transform.getOrigin());
00664 setCartPoseGoal(goal);
00665 return current_waypoint_;
00666 }
00667 else
00668 {
00669
00670 ROS_ERROR("Could not find sbpl plan");
00671 return 0;
00672 }
00673 }
00674
00675 btTransform tr, rel_tr;
00676 for (uint i = current_waypoint_; i < global_plan_.size(); ++i) {
00677 tf::poseMsgToTF(global_plan_[i].pose, tr);
00678 rel_tr = robot_pose_actual_.inverseTimes(tr);
00679 if (rel_tr.getOrigin().length() > min_dist && rel_tr.getOrigin().x()
00680 > min_x_component) {
00681 goal.setRotation(rel_tr.getRotation());
00682 goal.setOrigin(rel_tr.getOrigin());
00683 setCartPoseGoal(goal);
00684 return i;
00685 }
00686 }
00687
00688 goal.setRotation(rel_tr.getRotation());
00689 goal.setOrigin(btVector3(min_dist, 0, 0));
00690 setCartPoseGoal(goal);
00691 return 0;
00692 }
00693
00694
00695 double CartLocalPlanner::baseTwistFromError()
00696 {
00697 twist_base_->linear.x = robot_pose_error_.getOrigin().x() * K_trans_base_;
00698 twist_base_->linear.y = robot_pose_error_.getOrigin().y() * K_trans_base_;
00699 twist_base_->angular.z = tf::getYaw(robot_pose_error_.getRotation()) * K_rot_base_;
00700
00701 return mag(*twist_base_);
00702 }
00703
00704 double CartLocalPlanner::cartTwistFromError()
00705 {
00706
00707 twist_cart_.twist.linear.x = cart_pose_error_.x * K_trans_cart_;
00708 twist_cart_.twist.linear.y = cart_pose_error_.y * K_trans_cart_;
00709 twist_cart_.twist.angular.z = cart_pose_error_.theta * K_rot_cart_;
00710 twist_cart_.header.frame_id = "base_footprint";
00711 twist_cart_.header.stamp = ros::Time::now();
00712
00713 return mag(twist_cart_.twist);
00714 }
00715
00716 void CartLocalPlanner::setYawFromVec(const tf::Pose& pose1, const tf::Pose& pose2, tf::Pose& res)
00717 {
00718 res = pose1;
00719
00720 const btVector3 v = pose1.getOrigin() - pose2.getOrigin();
00721 double yaw = 0;
00722
00723 const double waypoint_dist = v.length();
00724 if (waypoint_dist < 0.01) {
00725
00726 ROS_WARN("WAYPOINTS TOO CLOSE - HOLDING YAW FIXED");
00727 yaw = tf::getYaw(pose1.getRotation());
00728 }
00729 else {
00730
00731 yaw = atan2(v.getY(), v.getX());
00732 }
00733
00734 btQuaternion q;
00735 q.setRPY(0, 0, yaw);
00736 res.setRotation(q);
00737 }
00738
00739 bool CartLocalPlanner::setPlan(
00740 const std::vector<gm::PoseStamped>& global_plan) {
00741 current_waypoint_ = 0;
00742 goal_reached_time_ = ros::Time::now();
00743 original_global_plan_ = global_plan;
00744 if (!transformGlobalPlan(*tf_, global_plan, *costmap_ros_,
00745 costmap_ros_->getGlobalFrameID(), global_plan_)) {
00746 ROS_ERROR("Could not transform the global plan to the frame of the controller");
00747 return false;
00748 }
00749 return true;
00750 }
00751
00752 bool CartLocalPlanner::transformGlobalPlan(const tf::TransformListener& tf,
00753 const std::vector<gm::PoseStamped>& global_plan,
00754 const costmap_2d::Costmap2DROS& costmap,
00755 const std::string& global_frame,
00756 std::vector<gm::PoseStamped>& transformed_plan) {
00757 if (!global_plan.size() > 0) {
00758 ROS_ERROR("Received plan with zero length");
00759 return false;
00760 }
00761 const gm::PoseStamped& pose = global_plan[0];
00762 transformed_plan.clear();
00763
00764 tf::StampedTransform transform;
00765
00766 try {
00767 tf.lookupTransform(global_frame, ros::Time(),
00768 pose.header.frame_id, pose.header.stamp,
00769 pose.header.frame_id, transform);
00770 }
00771 catch (const tf::TransformException& ex) {
00772 ROS_ERROR ("TF Exception while transforming global plan: %s", ex.what());
00773 return false;
00774 }
00775
00776 tf::Stamped<tf::Pose> tf_pose;
00777 gm::PoseStamped newer_pose;
00778
00779
00780
00781
00782
00783
00784 tf::Stamped<tf::Pose> last_pose;
00785
00786 poseStampedMsgToTF(pose, last_pose);
00787 last_pose.setData(transform * last_pose);
00788 last_pose.stamp_ = transform.stamp_;
00789 last_pose.frame_id_ = global_frame;
00790 poseStampedTFToMsg(last_pose, newer_pose);
00791 transformed_plan.push_back(newer_pose);
00792
00793
00794 int pts_skipped = 0;
00795 for (unsigned int i = 1; i < global_plan.size(); ++i) {
00796 poseStampedMsgToTF(global_plan[i], tf_pose);
00797 const tf::Pose tmp_pose = tf_pose.inverseTimes(last_pose);
00798 const double dist = tmp_pose.getOrigin().length();
00799 if (dist > SBPL_DTHRESH) {
00800
00801 tf_pose.setData(transform * tf_pose);
00802 if(!subscribe_sbpl_plan_)
00803
00804 setYawFromVec(tf_pose, last_pose, tf_pose);
00805
00806
00807 tf_pose.stamp_ = transform.stamp_;
00808 tf_pose.frame_id_ = global_frame;
00809 poseStampedTFToMsg(tf_pose, newer_pose);
00810 transformed_plan.push_back(newer_pose);
00811
00812 }
00813 else {
00814 pts_skipped++;
00815 }
00816 last_pose = tf_pose;
00817 }
00818 ROS_DEBUG_COND_NAMED(debug_print_, "transform_global_plan", "added %zu points (%d skipped)",
00819 global_plan.size()-pts_skipped, pts_skipped);
00820 return true;
00821 }
00822
00823 vector<gm::Point> CartLocalPlanner::transformFootprint(
00824 const gm::PolygonStamped& poly) const {
00825 vector<gm::Point> points;
00826 BOOST_FOREACH (const gm::Point32 p, poly.polygon.points)
00827 {
00828 gm::PointStamped in, out;
00829 in.header.frame_id = poly.header.frame_id;
00830 in.header.stamp = ros::Time();
00831 in.point.x = p.x;
00832 in.point.y = p.y;
00833 in.point.z = p.z;
00834 tf_->transformPoint(costmap_ros_->getGlobalFrameID(), in,
00835 out);
00836 points.push_back(out.point);
00837 }
00838 return points;
00839 }
00840
00841 void CartLocalPlanner::freeze()
00842 {
00843 ROS_WARN_THROTTLE(3.0, "Robot is in frozen state in cart local planner");
00844 gm::Twist empty_twist;
00845 twist_base_->linear.x = empty_twist.linear.x;
00846 twist_base_->linear.y = empty_twist.linear.y;
00847 twist_base_->angular.z = empty_twist.angular.z;
00848 twist_cart_.twist = empty_twist;
00849 twist_cart_.header.stamp = ros::Time::now();
00850
00851 cart_twist_pub_.publish(twist_cart_);
00852 }
00853
00854 void CartLocalPlanner::scaleTwist2D(gm::Twist &t, double scale)
00855 {
00856 t.linear.x *= scale;
00857 t.linear.y *= scale;
00858 t.angular.z *= scale;
00859 }
00860
00861 gm::Twist CartLocalPlanner::mapBaseTwistToCart(const gm::Twist &twist_base)
00862 {
00863 KDL::Frame cart_frame;
00864 tf::PoseTFToKDL(cart_pose_actual_.inverse(), cart_frame);
00865
00866 KDL::Twist twist_at_base;
00867 tf::TwistMsgToKDL(twist_base, twist_at_base);
00868
00869 KDL::Twist twist_at_cart = cart_frame * twist_at_base;
00870
00871 gm::Twist t;
00872 tf::TwistKDLToMsg(twist_at_cart, t);
00873 return t;
00874 }
00875
00876 void CartLocalPlanner::publishDebugPose(gm::Pose &p) {
00877 gm::Pose2D msg = gm::Pose2D();
00878 msg.x = p.position.x;
00879 msg.y = p.position.y;
00880 msg.theta = tf::getYaw(btQuaternion(p.orientation.x, p.orientation.y,
00881 p.orientation.z, p.orientation.w));
00882 pose2D_pub_.publish(msg);
00883 }
00884
00885 void CartLocalPlanner::publishDebugTwist(gm::Twist &t) {
00886 gm::Pose2D msg = gm::Pose2D();
00887 msg.x = t.linear.x;
00888 msg.y = t.linear.y;
00889 msg.theta = t.angular.z;
00890 pose2D_pub_.publish(msg);
00891 }
00892
00893 void CartLocalPlanner::publishDebugPose(tf::Pose &p) {
00894 gm::Pose2D msg = gm::Pose2D();
00895 msg.x = p.getOrigin().x();
00896 msg.y = p.getOrigin().y();
00897 msg.theta = tf::getYaw(p.getRotation());
00898 pose2D_pub_.publish(msg);
00899 }
00900
00901 void CartLocalPlanner::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
00902
00903 boost::mutex::scoped_lock lock(odom_lock_);
00904 base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
00905 base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
00906 base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
00907 }
00908
00909 void CartLocalPlanner::invalidPoseCallback (const std_msgs::Empty::ConstPtr& msg)
00910 {
00911 boost::mutex::scoped_lock lock(invalid_pose_mutex_);
00912 last_invalid_pose_time_ = ros::Time::now();
00913 }
00914
00915 bool CartLocalPlanner::isGoalReached() {
00916 if (!initialized_) {
00917 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00918 return false;
00919 }
00920
00921 bool quick_check = current_waypoint_ == global_plan_.size() - 1;
00922 if (!quick_check)
00923 return false;
00924 else {
00925 ROS_DEBUG_COND_NAMED(debug_print_, "is_goal_reached", "Quick check passed, checking actual goal distance");
00926
00927 nav_msgs::Odometry base_odom;
00928 {
00929 boost::recursive_mutex::scoped_lock(odom_lock_);
00930 base_odom = base_odom_;
00931 }
00932
00933 bool val = base_local_planner::isGoalReached(*tf_, global_plan_,
00934 *costmap_ros_, costmap_ros_->getGlobalFrameID(), base_odom,
00935 rot_stopped_velocity_, trans_stopped_velocity_,
00936 tolerance_trans_+0.1, tolerance_rot_+0.1);
00937 if (val) {
00938 ROS_DEBUG_COND_NAMED(debug_print_, "is_goal_reached", "Agreed with quick check... GOAL REACHED!");
00939 freeze();
00940 }
00941 else
00942 ROS_DEBUG("Tracking to last waypoint, should be at goal soon.");
00943
00944 return val;
00945 }
00946 }
00947
00948 };