00001
00002
00003
00004
00005
00006
00007
00008 #include "cart_local_planner/fixed_front_cart_planner.h"
00009 #include <pluginlib/class_list_macros.h>
00010
00011 PLUGINLIB_DECLARE_CLASS(cart_local_planner, FixedFrontCartPlanner, cart_local_planner::FixedFrontCartPlanner, nav_core::BaseLocalPlanner)
00012
00013 namespace cart_local_planner {
00014 FixedFrontCartPlanner::FixedFrontCartPlanner()
00015 {
00016 }
00017
00018 FixedFrontCartPlanner::~FixedFrontCartPlanner()
00019 {
00020 }
00021
00022 void FixedFrontCartPlanner::initialization_extras()
00023 {
00024 cart_max_y_offset_ = 0.1;
00025 y_compensation_gain_ = 5.0;
00026 extra_cart_collision_checker_ = cart_collision_checker_;
00027 extra_cart_collision_checker_.setPubTopic(std::string("extra_checker"));
00028 }
00029
00030 void FixedFrontCartPlanner::setControlMode()
00031 {
00032 if (cart_pose_goal_.getOrigin().y() - cart_pose_actual_.getOrigin().y() > cart_max_y_offset_)
00033 control_mode_ = RECOVERY;
00034 else if (fabs(tf::getYaw(robot_pose_error_.getRotation())) > M_PI_4 && control_mode_ != RECOVERY)
00035 control_mode_ = REGULAR;
00036 else
00037 control_mode_ = REGULAR;
00038
00039
00040
00041 ROS_DEBUG("mode = %d", control_mode_);
00042 }
00043
00044 void FixedFrontCartPlanner::controlModeAction()
00045 {
00046 switch (control_mode_) {
00047 case REGULAR:
00048 {
00049
00050 baseTwistFromError();
00051
00052
00053 cartTwistFromError();
00054
00055
00056 filterTwistsCombined(ALL);
00057 ROS_DEBUG("current_waypoint_ = %u", current_waypoint_);
00058 if (robot_pose_error_.getOrigin().length() < tolerance_trans_ && current_waypoint_ < global_plan_.size()-1)
00059 current_waypoint_++;
00060 }
00061 break;
00062
00063 case ROTATING_IN_PLACE:
00064 {
00065 ROS_WARN("NOT IMPLEMENTED");
00066 freeze();
00067 }
00068 break;
00069
00070 case RECOVERY:
00071 ROS_WARN("NOT IMPLEMENTED");
00072 freeze();
00073 break;
00074
00075 default:
00076 ROS_WARN("Unrecognized control mode requested");
00077 break;
00078 }
00079 }
00080
00081 void FixedFrontCartPlanner::filterTwistsCombined(int filter_options)
00082 {
00084 if (filter_options & GLOBAL_SCALING) {
00085 double xv_scale = fabs(twist_base_->linear.x) / twist_base_max_.linear.x;
00086 double yv_scale = fabs(twist_base_->linear.y) / twist_base_max_.linear.y;
00087 double tv_scale = fabs(twist_base_->angular.z) / twist_base_max_.angular.z;
00088
00089 double base_scaling_factor = std::max(xv_scale, std::max(yv_scale, tv_scale));
00090
00091 double xv_cart_scale = fabs(twist_cart_.twist.linear.x) / twist_cart_max_.linear.x;
00092 double yv_cart_scale = fabs(twist_cart_.twist.linear.y) / twist_cart_max_.linear.y;
00093 double tv_cart_scale = fabs(twist_cart_.twist.angular.z)/ twist_cart_max_.angular.z;
00094
00095 double cart_scaling_factor = std::max(xv_cart_scale, std::max(yv_cart_scale, tv_cart_scale));
00096 double scaling_factor = std::max(base_scaling_factor, cart_scaling_factor);
00097
00098
00099 if (scaling_factor > 1.0) {
00100 double scale_mult = 1.0 / scaling_factor;
00101 scaleTwist2D(*twist_base_, scale_mult);
00102 scaleTwist2D(twist_cart_.twist, scale_mult);
00103 }
00104 }
00105
00107 if (filter_options & COMPENSATE_BASE_TWIST) {
00108 geometry_msgs::Twist base_twist_at_cart = mapBaseTwistToCart(*twist_base_);
00109 twist_cart_.twist.angular.z -= base_twist_at_cart.angular.z;
00110 }
00111
00113 if (filter_options & HOLONOMIC_CONSTRAINT) {
00114
00115 geometry_msgs::Twist base_twist_at_cart = mapBaseTwistToCart(*twist_base_);
00116 geometry_msgs::Twist twist_net = base_twist_at_cart + twist_cart_.twist;
00117 extra_cart_collision_checker_.checkTwist(twist_net, num_traj_steps_, dt_, true, false);
00118
00119
00120 static int idx = 0;
00121 if (idx % 20 == 0) {
00122 ros::param::get("/ygain", y_compensation_gain_);
00123 ROS_WARN("fetched new ygain from param server: %lf", y_compensation_gain_);
00124 }
00125 ++idx;
00126
00127 ROS_INFO("cart_pose_error_.y = %.3lf", cart_pose_error_.y);
00128 twist_cart_.twist.linear.y = 0;
00129 ROS_INFO("twist.a.z old = %.3lf", twist_cart_.twist.angular.z);
00130 twist_cart_.twist.angular.z += cart_pose_error_.y * y_compensation_gain_;
00131 ROS_INFO("twist.a.z new = %.3lf", twist_cart_.twist.angular.z);
00132 }
00133
00135 if (filter_options & CART_ERR_SCALING) {
00136 ROS_DEBUG("cart twist mag = %.3lf, gaussian scaling factor = %.3lf", mag(twist_cart_.twist), pow(M_E, -50.0 * pow(mag(twist_cart_.twist), 2)));
00137 scaleTwist2D(*twist_base_, pow(M_E, -50.0 * pow(mag(twist_cart_.twist), 2)));
00138
00139 }
00140 }
00141
00142 };