cart_local_planner: cart_local_planner: holonomic_cart

00001 /*
00002  * holonomic_cart_planner.cpp
00003  *
00004  *  Created on: Aug 17, 2010
00005  *      Author: jscholz
00006  */
00007 
00008 #include <cart_local_planner/holonomic_cart_planner.h>
00009 #include <cart_local_planner/utils.h>
00010 #include <pluginlib/class_list_macros.h>
00011 
00012 PLUGINLIB_DECLARE_CLASS(cart_local_planner, HolonomicCartPlanner, cart_local_planner::HolonomicCartPlanner, nav_core::BaseLocalPlanner)
00013 
00014 namespace cart_local_planner {
00015 HolonomicCartPlanner::HolonomicCartPlanner()
00016 {
00017 }
00018 
00019 void HolonomicCartPlanner::initialization_extras()
00020 {
00021 }
00022 
00023 HolonomicCartPlanner::~HolonomicCartPlanner() {
00024 }
00025 
00026 void HolonomicCartPlanner::setControlMode()
00027 {
00028   // double yaw_err = tf::getYaw(robot_pose_error_.getRotation());
00029   boost::mutex::scoped_lock lock(invalid_pose_mutex_);    
00030   if (ros::Time::now() - last_invalid_pose_time_ < ros::Duration(3.0)) 
00031     control_mode_ = PULLING_ARMS_IN;
00032   //  else if (fabs(yaw_err) > M_PI_4 && control_mode_ != RECOVERY)
00033   //    control_mode_ = ROTATING_IN_PLACE;
00034   else
00035     control_mode_ = REGULAR;
00036 
00037   ROS_DEBUG_COND_NAMED(debug_print_, "mode", "mode = %d", control_mode_);
00038 }
00039 
00040 void HolonomicCartPlanner::controlModeAction()
00041 {
00042   switch (control_mode_) {
00043 
00044 
00045   case PULLING_ARMS_IN:
00046     {
00047       /* 2010/9/3: Disabling to test the articulate cart server recovery
00048       ROS_DEBUG_STREAM_NAMED ("recovery", "In pulling-arms-in recovery; current cart pose goal is " <<
00049                               toString(cart_pose_goal_) << "; robot pose goal is " << toString(robot_pose_goal_));
00050       // Reset the cart goal pose to be close to the robot
00051       tf::Stamped<tf::Pose> goal;
00052       goal.setIdentity();
00053       goal.getOrigin().setX(0.5*(cart_range.x_min+cart_range.x_max));
00054       setRobotPoseGoal(robot_pose_actual_);;
00055       setCartPoseGoal(goal);
00056       ROS_DEBUG_STREAM_NAMED ("recovery", "  Modified cart pose goal to " << toString(cart_pose_goal_) <<
00057                               " and robot goal to " << toString(robot_pose_goal_));
00058       */
00059 
00060     }
00061     // We don't break, but continue with the regular behavior with the modified goals
00062     
00063   case REGULAR:
00064     {
00065       // Compute base twist
00066       baseTwistFromError();
00067 
00068       // Compute cart twist
00069       cartTwistFromError();
00070 
00071       // Coordinate base and cart velocities for smooth and safe action
00072       filterTwistsCombined(GLOBAL_SCALING);
00073 
00074       if (robot_pose_error_.getOrigin().length() < tolerance_trans_ && current_waypoint_ < global_plan_.size()-1 && mag(cart_pose_error_) < 0.1)
00075         current_waypoint_++;
00076     }
00077     break;
00078 
00079   case ROTATING_IN_PLACE:
00080     {
00081       cartTwistFromError();
00082       baseTwistFromError();
00083 
00084       // Coordinate base and cart velocities for smooth and safe action
00085       filterTwistsCombined(GLOBAL_SCALING);
00086     }
00087     break;
00088 
00089   case RECOVERY:
00090     ROS_WARN("NOT IMPLEMENTED");
00091     break;
00092 
00093   default:
00094     ROS_WARN("Unrecognized control mode requested");
00095     break;
00096   }
00097 }
00098 
00099 void HolonomicCartPlanner::filterTwistsCombined(int filter_options)
00100 {
00102   if (filter_options & GLOBAL_SCALING) {
00103     double xv_scale = fabs(twist_base_->linear.x) / twist_base_max_.linear.x;
00104     double yv_scale = fabs(twist_base_->linear.y) / twist_base_max_.linear.y;
00105     double tv_scale = fabs(twist_base_->angular.z) / twist_base_max_.angular.z;
00106 
00107     double base_scaling_factor = std::max(xv_scale, std::max(yv_scale, tv_scale));
00108 
00109     double xv_cart_scale = fabs(twist_cart_.twist.linear.x) / twist_cart_max_.linear.x;
00110     double yv_cart_scale = fabs(twist_cart_.twist.linear.y) / twist_cart_max_.linear.y;
00111     double tv_cart_scale = fabs(twist_cart_.twist.angular.z)/ twist_cart_max_.angular.z;
00112 
00113     double cart_scaling_factor = std::max(xv_cart_scale, std::max(yv_cart_scale, tv_cart_scale));
00114     double scaling_factor = std::max(base_scaling_factor, cart_scaling_factor);
00115 
00116     // Scales both twists together such that none are above their limits
00117     if (scaling_factor > 1.0) {
00118       double scale_mult = 1.0 / scaling_factor;
00119       scaleTwist2D(*twist_base_, scale_mult); //TODO verify that this works
00120       scaleTwist2D(twist_cart_.twist, scale_mult);
00121       ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "twist_filter",
00122                                    "Scaling, to keep things in range, cart and base twists by " << scale_mult);
00123     }
00124 
00125   }
00126 
00128   if (filter_options & CART_ERR_SCALING) {
00129     const double scaling_factor = pow(M_E, -50.0 * pow(mag(twist_cart_.twist), 2)); // tuned to fall off around 0.1
00130     scaleTwist2D(*twist_base_, scaling_factor);
00131     ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "twist_filter",
00132                                  "Scaling, based on cart error, base velocity by " << scaling_factor);
00133   }
00134 
00136   if (filter_options & COMPENSATE_BASE_TWIST) {
00137     geometry_msgs::Twist base_twist_at_cart = mapBaseTwistToCart(*twist_base_);
00138     ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "twist_filter", "Cart twist " << twist_cart_ <<
00139                                  " and base twist " << base_twist_at_cart);
00140     twist_cart_.twist.linear.y -= base_twist_at_cart.linear.y;
00141     twist_cart_.twist.angular.z -= base_twist_at_cart.angular.z;
00142     ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "twist_filter", "After compensating for base twist, cart twist is "
00143                                  << twist_cart_);
00144   }
00145 
00147   if (filter_options & GLOBAL_SCALING) {
00148     double xv_scale = fabs(twist_base_->linear.x) / twist_base_max_.linear.x;
00149     double yv_scale = fabs(twist_base_->linear.y) / twist_base_max_.linear.y;
00150     double tv_scale = fabs(twist_base_->angular.z) / twist_base_max_.angular.z;
00151 
00152     double base_scaling_factor = std::max(xv_scale, std::max(yv_scale, tv_scale));
00153 
00154     double xv_cart_scale = fabs(twist_cart_.twist.linear.x) / twist_cart_max_.linear.x;
00155     double yv_cart_scale = fabs(twist_cart_.twist.linear.y) / twist_cart_max_.linear.y;
00156     double tv_cart_scale = fabs(twist_cart_.twist.angular.z)/ twist_cart_max_.angular.z;
00157 
00158     double cart_scaling_factor = std::max(xv_cart_scale, std::max(yv_cart_scale, tv_cart_scale));
00159     double scaling_factor = std::max(base_scaling_factor, cart_scaling_factor);
00160 
00161     // Scales both twists together such that none are above their limits
00162     if (scaling_factor > 1.0) {
00163       double scale_mult = 1.0 / scaling_factor;
00164       scaleTwist2D(*twist_base_, scale_mult); //TODO verify that this works
00165       scaleTwist2D(twist_cart_.twist, scale_mult);
00166       ROS_DEBUG_STREAM_COND_NAMED (debug_print_, "twist_filter",
00167                                    "Scaling, to keep things in range, cart and base twists by " << scale_mult);
00168     }
00169 
00170   }
00171 
00172 }
00173 
00174 }; // namespace cart_local_planner