00001
00002
00003
00004
00005
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
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
00033
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
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 }
00061
00062
00063 case REGULAR:
00064 {
00065
00066 baseTwistFromError();
00067
00068
00069 cartTwistFromError();
00070
00071
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
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
00117 if (scaling_factor > 1.0) {
00118 double scale_mult = 1.0 / scaling_factor;
00119 scaleTwist2D(*twist_base_, scale_mult);
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));
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
00162 if (scaling_factor > 1.0) {
00163 double scale_mult = 1.0 / scaling_factor;
00164 scaleTwist2D(*twist_base_, scale_mult);
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 };