00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00042 #ifndef HOLONOMIC_CART_PLANNER_H_ 00043 #define HOLONOMIC_CART_PLANNER_H_ 00044 00045 #include "cart_local_planner/cart_local_planner.h" 00046 00047 /* 00048 ** TODO write a CLP for steel cart? 00049 It'd need to have base vel be subordinate to cart vel, and have a recovery behavior of some sort: 00050 1: get target cart pose as usual 00051 2: get the required yaw to move the cart in this direction (assumes we have mobility around main axis) 00052 a) if the angle is +- 90, then we're in "forward" mode, and we can set this cart angle and drive the base forward 00053 b) if the angle is > +-90, then we're past the target waypoint, and we have to 00054 3) we'll often have a Y offset between the cart and the target waypoint which we need to decide how to fix. 00055 Ideally we'd decide to recover for the current point or for a future point based on how much space we have ahead 00056 For now, the simplest thing is to have a recovery mode in which the cart jockeys back and forth to pull the cart back to the target pose. 00057 We can accomplish this with the arms, while incrementing and decrementing the waypoint counter by some amount (*must allow negative base vels) 00058 --------------- 00059 check y error 00060 if greater than a threshold, we're in recovery mode 00061 else, we're in regular drive forward mode. 00062 threshold is whatever the maximum y offset the cart can have where the robot is still capable of pointing it back towards the middle 00063 00064 00065 drive forward mode: main problem is that cart has to be pointed towards the plan before we drive forward 00066 pick a cart angle so that the yerr is elimated in N steps, where N is a customizable param that determines how conservative the controller is. 00067 This param is what we'd control if we want it to be lazy in an open room, but careful in tight spaces. Since this is getting called every loop, 00068 it'll actually take way longer than N, but will conveniently decay to zero yaw and tend to minimize yerr. 00069 00070 recovery mode: 00071 00072 ---------- 00073 what's different from the holonomic case: 00074 00075 1: base vel is strictly subordinate to cart vel 00076 -> solution: make virtual func for base scaling from cart err 00077 00078 2: modes depend on y error and yaw error\ 00079 ---------- 00080 to implement: 00081 option 1: override the setGoals funciton to compute a target cart pose that points towards the plan, and always has a y-error of zero. then add a filter 00082 operation that stops the base unless the cart error is basically zero 00083 00084 option 2: try to do everything at the twist level, by filtering out the linear.y component and biasing the angular.z component such that the y 00085 error will tend to reduce. 00086 00087 option 2 is simpler, but dumber and tougher to tune 00088 */ 00089 00090 namespace cart_local_planner { 00091 00092 class FixedFrontCartPlanner: public CartLocalPlanner { 00093 public: 00094 FixedFrontCartPlanner(); 00095 virtual ~FixedFrontCartPlanner(); 00096 00097 protected: 00102 virtual void initialization_extras(); 00103 00104 /************ 00105 * PLANNING * 00106 ************/ 00110 virtual void setControlMode(); 00111 00118 virtual void controlModeAction(); 00119 00124 virtual void filterTwistsCombined(int filter_options); 00125 00131 double pointCartAtTarget(tf::Pose &point_pose); 00132 00133 /**************** 00134 * DATA MEMBERS * 00135 ****************/ 00136 enum CONTROL_MODE { 00137 REGULAR, ROTATING_IN_PLACE, RECOVERY 00138 }; 00139 enum CONTROL_MODE control_mode_; 00140 00141 enum FILTER_OPTIONS { 00142 GLOBAL_SCALING = 0x1, 00143 CART_ERR_SCALING = GLOBAL_SCALING << 1, 00144 COMPENSATE_BASE_TWIST = GLOBAL_SCALING << 2, 00145 HOLONOMIC_CONSTRAINT = GLOBAL_SCALING << 3, 00146 ALL = 0xffff 00147 }; 00148 00149 double cart_max_y_offset_; 00150 double y_compensation_gain_; 00151 00152 CostmapTrajectoryChecker extra_cart_collision_checker_; 00153 }; 00154 00155 } 00156 ; // namespace cart_local_planner 00157 00158 #endif /* FIXED_FRONT_CART_PLANNER_H_ */