eband_trajectory_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2010, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Christian Connette
00036 *********************************************************************/
00037 
00038 #include <eband_local_planner/eband_trajectory_controller.h>
00039 #include <tf/transform_datatypes.h>
00040 
00041 
00042 namespace eband_local_planner{
00043 
00044 using std::min;
00045 using std::max;
00046 
00047 
00048 EBandTrajectoryCtrl::EBandTrajectoryCtrl() : costmap_ros_(NULL), initialized_(false), band_set_(false), visualization_(false) {}
00049 
00050 
00051 EBandTrajectoryCtrl::EBandTrajectoryCtrl(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00052   : costmap_ros_(NULL), initialized_(false), band_set_(false), visualization_(false)
00053 {
00054         // initialize planner
00055         initialize(name, costmap_ros);
00056 
00057   // Initialize pid object (note we'll be further clamping its output)
00058   pid_.initPid(1, 0, 0, 10, -10);
00059 }
00060 
00061 
00062 EBandTrajectoryCtrl::~EBandTrajectoryCtrl() {}
00063 
00064 
00065 void EBandTrajectoryCtrl::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00066 {
00067 
00068         // check if trajectory controller is already initialized
00069         if(!initialized_)
00070         {
00071                 // create Node Handle with name of plugin (as used in move_base for loading)
00072                 ros::NodeHandle node_private("~/" + name);
00073         
00074                 // read parameters from parameter server
00075                 node_private.param("max_vel_lin", max_vel_lin_, 0.6);
00076                 node_private.param("max_vel_th", max_vel_th_, 1.5);
00077 
00078                 node_private.param("min_vel_lin", min_vel_lin_, 0.1);
00079                 node_private.param("min_vel_th", min_vel_th_, 0.0);
00080 
00081                 node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
00082                 node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
00083 
00084                 node_private.param("tolerance_trans", tolerance_trans_, 0.02);
00085                 node_private.param("tolerance_rot", tolerance_rot_, 0.04);
00086                 node_private.param("tolerance_timeout", tolerance_timeout_, 0.5);
00087 
00088                 node_private.param("k_prop", k_p_, 4.0);
00089                 node_private.param("k_damp", k_nu_, 3.5);
00090                 node_private.param("Ctrl_Rate", ctrl_freq_, 10.0); // TODO retrieve this from move base parameters
00091 
00092                 node_private.param("max_acceleration", acc_max_, 0.5);
00093                 node_private.param("virtual_mass", virt_mass_, 0.75);
00094 
00095                 node_private.param("max_translational_acceleration", acc_max_trans_, 0.5);
00096                 node_private.param("max_rotational_acceleration", acc_max_rot_, 1.5);
00097 
00098                 node_private.param("rotation_correction_threshold", rotation_correction_threshold_, 0.5);
00099 
00100                 // copy adress of costmap and Transform Listener (handed over from move_base)
00101                 costmap_ros_ = costmap_ros;
00102 
00103                 // init velocity for interpolation
00104                 last_vel_.linear.x = 0.0;
00105                 last_vel_.linear.y = 0.0;
00106                 last_vel_.linear.z = 0.0;
00107                 last_vel_.angular.x = 0.0;
00108                 last_vel_.angular.y = 0.0;
00109                 last_vel_.angular.z = 0.0;
00110 
00111                 // set the general refernce frame to that in which the band is given
00112                 geometry_msgs::Pose2D tmp_pose2D;
00113                 tmp_pose2D.x = 0.0;
00114                 tmp_pose2D.y = 0.0;
00115                 tmp_pose2D.theta = 0.0;
00116                 Pose2DToPose(ref_frame_band_, tmp_pose2D);
00117 
00118                 // set initialized flag
00119                 initialized_ = true;
00120         }
00121         else
00122         {
00123                 ROS_WARN("This planner has already been initialized, doing nothing.");
00124         }
00125 }
00126 
00127 
00128 void EBandTrajectoryCtrl::setVisualization(boost::shared_ptr<EBandVisualization> target_visual)
00129 {
00130         target_visual_ = target_visual;
00131 
00132         visualization_ = true;
00133 }
00134 
00135 bool EBandTrajectoryCtrl::setBand(const std::vector<Bubble>& elastic_band)
00136 {
00137         elastic_band_ = elastic_band;
00138         band_set_ = true;
00139         return true;
00140 }
00141 
00142 
00143 bool EBandTrajectoryCtrl::setOdometry(const nav_msgs::Odometry& odometry)
00144 {
00145         odom_vel_.linear.x = odometry.twist.twist.linear.x;
00146         odom_vel_.linear.y = odometry.twist.twist.linear.y;
00147         odom_vel_.linear.z = 0.0;
00148         odom_vel_.angular.x = 0.0;
00149         odom_vel_.angular.y = 0.0;
00150         odom_vel_.angular.z = odometry.twist.twist.angular.z;
00151 
00152         return true;
00153 }
00154 
00155 // Return the angular difference between the direction we're pointing
00156 // and the direction we want to move in
00157 double angularDiff (const geometry_msgs::Twist& heading,
00158                     const geometry_msgs::Pose& pose)
00159 {
00160   const double pi = 3.14159265;
00161   const double t1 = atan2(heading.linear.y, heading.linear.x);
00162   const double t2 = tf::getYaw(pose.orientation);
00163   const double d = t1-t2;
00164 
00165   if (fabs(d)<pi)
00166     return d;
00167   else if (d<0)
00168     return d+2*pi;
00169   else
00170     return d-2*pi;
00171 }
00172 
00173 
00174 bool EBandTrajectoryCtrl::getTwist(geometry_msgs::Twist& twist_cmd)
00175 {
00176         // init twist cmd to be handed back to caller
00177         geometry_msgs::Twist robot_cmd, bubble_diff, control_deviation;
00178         robot_cmd.linear.x = 0.0;
00179         robot_cmd.linear.y = 0.0;
00180         robot_cmd.linear.z = 0.0;
00181         robot_cmd.angular.x = 0.0;
00182         robot_cmd.angular.y = 0.0;
00183         robot_cmd.angular.z = 0.0;
00184 
00185         // make sure command vector is clean
00186         twist_cmd = robot_cmd;
00187         control_deviation = robot_cmd;
00188 
00189         // check if plugin initialized
00190         if(!initialized_)
00191         {
00192                 ROS_ERROR("Requesting feedforward command from not initialized planner. Please call initialize() before using this planner");
00193                 return false;
00194         }
00195 
00196         // check there is a plan at all (minimum 1 frame in this case, as robot + goal = plan)
00197         if( (!band_set_) || (elastic_band_.size() < 2) )
00198         {
00199                 ROS_WARN("Requesting feedforward command from empty band.");
00200                 return false;
00201         }
00202 
00203         // calc intersection of bubble-radius with sequence of vector connecting the bubbles
00204 
00205         // get distance to target from bubble-expansion
00206         double scaled_radius = 0.7 * elastic_band_.at(0).expansion;
00207 
00208         // get difference and distance between bubbles in odometry frame
00209         double bubble_distance, ang_pseudo_dist;
00210         bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose,
00211                                                                                                 elastic_band_.at(1).center.pose,
00212                                                                                                         ref_frame_band_);
00213         ang_pseudo_dist = bubble_diff.angular.z * costmap_ros_->getCircumscribedRadius();
00214         bubble_distance = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) + (bubble_diff.linear.y * bubble_diff.linear.y) +
00215                                                 (ang_pseudo_dist * ang_pseudo_dist) );
00216 
00217         if(visualization_)
00218         {
00219                 target_visual_->publishBubble("ctrl_target", 1, target_visual_->blue, elastic_band_.at(0));
00220                 target_visual_->publishBubble("ctrl_target", 2, target_visual_->blue, elastic_band_.at(1));
00221         }
00222 
00223         // by default our control deviation is the difference between the bubble centers
00224         double abs_ctrl_dev;
00225         control_deviation = bubble_diff;
00226 
00227 
00228         ang_pseudo_dist = control_deviation.angular.z * costmap_ros_->getCircumscribedRadius();
00229         abs_ctrl_dev = sqrt( (control_deviation.linear.x * control_deviation.linear.x) +
00230                                                                 (control_deviation.linear.y * control_deviation.linear.y) +
00231                                                                         (ang_pseudo_dist * ang_pseudo_dist) );
00232 
00233         // yet depending on the expansion of our bubble we might want to adapt this point
00234         if(scaled_radius < bubble_distance)
00235         {
00236                 // triviale case - simply scale bubble_diff
00237                 double scale_difference = scaled_radius / bubble_distance;
00238                 bubble_diff.linear.x *= scale_difference;
00239                 bubble_diff.linear.y *= scale_difference;
00240                 bubble_diff.angular.z *= scale_difference;
00241                 // set controls
00242                 control_deviation = bubble_diff;
00243         }
00244         
00245         // if scaled_radius = bubble_distance -- we have nothing to do at all
00246 
00247         if(scaled_radius > bubble_distance)
00248         {
00249                 // o.k. now we have to do a little bit more -> check next but one bubble
00250                 if(elastic_band_.size() > 2)
00251                 {
00252                         // get difference between next and next but one bubble
00253                         double next_bubble_distance;
00254                         geometry_msgs::Twist next_bubble_diff;
00255                         next_bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(1).center.pose,
00256                                                                                                                         elastic_band_.at(2).center.pose,
00257                                                                                                                                 ref_frame_band_);
00258                         ang_pseudo_dist = next_bubble_diff.angular.z * costmap_ros_->getCircumscribedRadius();
00259                         next_bubble_distance = sqrt( (next_bubble_diff.linear.x * next_bubble_diff.linear.x) +
00260                                                                                         (next_bubble_diff.linear.y * next_bubble_diff.linear.y) +
00261                                                                                                 (ang_pseudo_dist * ang_pseudo_dist) );
00262 
00263                         if(scaled_radius > (bubble_distance + next_bubble_distance) )
00264                         {
00265                                 // we should normally not end up here - but just to be sure
00266                                 control_deviation.linear.x = bubble_diff.linear.x + next_bubble_diff.linear.x;
00267                                 control_deviation.linear.y = bubble_diff.linear.y + next_bubble_diff.linear.y;
00268                                 control_deviation.angular.z = bubble_diff.angular.z + next_bubble_diff.angular.z;
00269                                 // done
00270                                 if(visualization_)
00271                                         target_visual_->publishBubble("ctrl_target", 3, target_visual_->red, elastic_band_.at(2));
00272                         }
00273                         else
00274                         {
00275                                 if(visualization_)
00276                                         target_visual_->publishBubble("ctrl_target", 3, target_visual_->red, elastic_band_.at(2));
00277 
00278                                 // we want to calculate intersection point of bubble ...
00279                                 // ... and vector connecting the following bubbles
00280                                 double b_distance, cosine_at_bub;
00281                                 double vec_prod, norm_vec1, norm_vec2;
00282                                 double ang_pseudo_dist1, ang_pseudo_dist2;
00283 
00284                                 // get distance between next bubble center and intersection point
00285                                 ang_pseudo_dist1 = bubble_diff.angular.z * costmap_ros_->getCircumscribedRadius();
00286                                 ang_pseudo_dist2 = next_bubble_diff.angular.z * costmap_ros_->getCircumscribedRadius();
00287                                 // careful! - we need this sign because of the direction of the vectors and the definition of the vector-product
00288                                 vec_prod = - ( (bubble_diff.linear.x * next_bubble_diff.linear.x) +
00289                                                                 (bubble_diff.linear.y * next_bubble_diff.linear.y) +
00290                                                                         (ang_pseudo_dist1 * ang_pseudo_dist2) );
00291 
00292                                 norm_vec1 = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) +
00293                                                                 (bubble_diff.linear.y * bubble_diff.linear.y) +
00294                                                                         (ang_pseudo_dist1 * ang_pseudo_dist1) );
00295 
00296                                 norm_vec2 = sqrt( (next_bubble_diff.linear.x * next_bubble_diff.linear.x) +
00297                                                                 (next_bubble_diff.linear.y * next_bubble_diff.linear.y) +
00298                                                                         (ang_pseudo_dist2 * ang_pseudo_dist2) );
00299 
00300                                 // reform the cosine-rule
00301                                 cosine_at_bub = vec_prod / norm_vec1 / norm_vec2;
00302                                 b_distance = bubble_distance * cosine_at_bub + sqrt( scaled_radius*scaled_radius -
00303                                                                 bubble_distance*bubble_distance * (1.0 - cosine_at_bub*cosine_at_bub) );
00304 
00305                                 // get difference vector from next_bubble to intersection point
00306                                 double scale_next_difference = b_distance / next_bubble_distance;
00307                                 next_bubble_diff.linear.x *= scale_next_difference;
00308                                 next_bubble_diff.linear.y *= scale_next_difference;
00309                                 next_bubble_diff.angular.z *= scale_next_difference;
00310 
00311                                 // and finally get the control deviation
00312                                 control_deviation.linear.x = bubble_diff.linear.x + next_bubble_diff.linear.x;
00313                                 control_deviation.linear.y = bubble_diff.linear.y + next_bubble_diff.linear.y;
00314                                 control_deviation.angular.z = bubble_diff.angular.z + next_bubble_diff.angular.z;
00315                                 // done
00316                         }
00317                 }
00318         }
00319 
00320         // plot control deviation
00321         ang_pseudo_dist = control_deviation.angular.z * costmap_ros_->getCircumscribedRadius();
00322         abs_ctrl_dev = sqrt( (control_deviation.linear.x * control_deviation.linear.x) +
00323                                                                 (control_deviation.linear.y * control_deviation.linear.y) +
00324                                                                         (ang_pseudo_dist * ang_pseudo_dist) );
00325 
00326 
00327         if(visualization_)
00328         {
00329                 // compose bubble from ctrl-target
00330                 geometry_msgs::Pose2D tmp_bubble_2d, curr_bubble_2d;
00331                 geometry_msgs::Pose tmp_pose;
00332                 // init bubble for visualization
00333                 Bubble new_bubble = elastic_band_.at(0);
00334                 PoseToPose2D(elastic_band_.at(0).center.pose, curr_bubble_2d);
00335                 tmp_bubble_2d.x = curr_bubble_2d.x + control_deviation.linear.x;
00336                 tmp_bubble_2d.y = curr_bubble_2d.y + control_deviation.linear.y;
00337                 tmp_bubble_2d.theta = curr_bubble_2d.theta + control_deviation.angular.z;
00338                 Pose2DToPose(tmp_pose, tmp_bubble_2d);
00339                 new_bubble.center.pose = tmp_pose;
00340                 new_bubble.expansion = 0.1; // just draw a small bubble
00341                 target_visual_->publishBubble("ctrl_target", 0, target_visual_->red, new_bubble);
00342         }
00343 
00344 
00345         const geometry_msgs::Point& goal = (--elastic_band_.end())->center.pose.position;
00346         const double dx = elastic_band_.at(0).center.pose.position.x - goal.x;
00347         const double dy = elastic_band_.at(0).center.pose.position.y - goal.y;
00348         const double dist_to_goal = sqrt(dx*dx + dy*dy);
00349         
00350         // Assuming we're far enough from the final goal, make sure to rotate so
00351         // we're facing the right way
00352         if (dist_to_goal > rotation_correction_threshold_)
00353         {
00354         
00355           const double angular_diff = angularDiff(control_deviation, elastic_band_.at(0).center.pose);
00356           const double vel = pid_.updatePid(-angular_diff, ros::Duration(1/ctrl_freq_));
00357           const double mult = fabs(vel) > max_vel_th_ ? max_vel_th_/fabs(vel) : 1.0;
00358           control_deviation.angular.z = vel*mult;
00359           const double abs_vel = fabs(control_deviation.angular.z);
00360 
00361           ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction",
00362                                     "Angular diff is %.2f and desired angular "
00363                                     "vel is %.2f.  Initial translation velocity "
00364                                     "is %.2f, %.2f", angular_diff,
00365                                     control_deviation.angular.z,
00366                                     control_deviation.linear.x,
00367                                     control_deviation.linear.y);
00368           const double trans_mult = max(0.01, 1.0 - abs_vel/max_vel_th_); // There are some weird tf errors if I let it be 0
00369           control_deviation.linear.x *= trans_mult;
00370           control_deviation.linear.y *= trans_mult;
00371           ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction",
00372                                     "Translation multiplier is %.2f and scaled "
00373                                     "translational velocity is %.2f, %.2f",
00374                                     trans_mult, control_deviation.linear.x,
00375                                     control_deviation.linear.y);
00376         }
00377         else
00378           ROS_DEBUG_THROTTLE_NAMED (1.0, "angle_correction",
00379                                     "Not applying angle correction because "
00380                                     "distance to goal is %.2f", dist_to_goal);
00381                                     
00382         
00383 
00384 
00385         // now the actual control procedure start (using attractive Potentials)
00386         geometry_msgs::Twist desired_velocity, currbub_maxvel_dir;
00387         double desvel_abs, desvel_abs_trans, currbub_maxvel_abs;
00388         double scale_des_vel;
00389         desired_velocity = robot_cmd;
00390         currbub_maxvel_dir = robot_cmd;
00391         
00392         // calculate "equilibrium velocity" (Khatib86 - Realtime Obstacle Avoidance)
00393         desired_velocity.linear.x = k_p_/k_nu_ * control_deviation.linear.x;
00394         desired_velocity.linear.y = k_p_/k_nu_ * control_deviation.linear.y;
00395         desired_velocity.angular.z = k_p_/k_nu_ * control_deviation.angular.z;
00396 
00397         //robot_cmd = desired_velocity;
00398 
00399         // get max vel for current bubble
00400         int curr_bub_num = 0;
00401         currbub_maxvel_abs = getBubbleTargetVel(curr_bub_num, elastic_band_, currbub_maxvel_dir);
00402 
00403         // if neccessarry scale desired vel to stay lower than currbub_maxvel_abs
00404         ang_pseudo_dist = desired_velocity.angular.z * costmap_ros_->getCircumscribedRadius();
00405         desvel_abs = sqrt( (desired_velocity.linear.x * desired_velocity.linear.x) +
00406                                                         (desired_velocity.linear.y * desired_velocity.linear.y) +
00407                                                                 (ang_pseudo_dist * ang_pseudo_dist) );
00408         if(desvel_abs > currbub_maxvel_abs)
00409         {
00410                 scale_des_vel = currbub_maxvel_abs / desvel_abs;
00411                 desired_velocity.linear.x *= scale_des_vel;
00412                 desired_velocity.linear.y *= scale_des_vel;
00413                 desired_velocity.angular.z *= scale_des_vel;
00414         }
00415 
00416         // make sure to stay within velocity bounds for the robot
00417         desvel_abs_trans = sqrt( (desired_velocity.linear.x * desired_velocity.linear.x) + (desired_velocity.linear.y * desired_velocity.linear.y) );
00418         // for translation
00419         if(desvel_abs_trans > max_vel_lin_)
00420         {
00421                 scale_des_vel = max_vel_lin_ / desvel_abs_trans;
00422                 desired_velocity.linear.x *= scale_des_vel;
00423                 desired_velocity.linear.y *= scale_des_vel;
00424                 // to make sure we are staying inside the bubble also scale rotation
00425                 desired_velocity.angular.z *= scale_des_vel;
00426         }
00427 
00428         // for rotation
00429         if(fabs(desired_velocity.angular.z) > max_vel_th_)
00430         {
00431                 scale_des_vel = max_vel_th_ / fabs(desired_velocity.angular.z);
00432                 desired_velocity.angular.z *= scale_des_vel;
00433                 // to make sure we are staying inside the bubble also scale translation
00434                 desired_velocity.linear.x *= scale_des_vel;
00435                 desired_velocity.linear.y *= scale_des_vel;
00436         }
00437 
00438         // calculate resulting force (accel. resp.) (Khatib86 - Realtime Obstacle Avoidance)
00439         geometry_msgs::Twist acc_desired;
00440         acc_desired = robot_cmd;
00441         acc_desired.linear.x = (1.0/virt_mass_) * k_nu_ * (desired_velocity.linear.x - last_vel_.linear.x);
00442         acc_desired.linear.y = (1.0/virt_mass_) * k_nu_ * (desired_velocity.linear.y - last_vel_.linear.y);
00443         acc_desired.angular.z = (1.0/virt_mass_) * k_nu_ * (desired_velocity.angular.z - last_vel_.angular.z);
00444 
00445         // constrain acceleration
00446         double scale_acc;
00447         double abs_acc_trans = sqrt( (acc_desired.linear.x*acc_desired.linear.x) + (acc_desired.linear.y*acc_desired.linear.y) );
00448         if(abs_acc_trans > acc_max_trans_)
00449         {
00450                 scale_acc = acc_max_trans_ / abs_acc_trans;
00451                 acc_desired.linear.x *= scale_acc;
00452                 acc_desired.linear.y *= scale_acc;
00453                 // again - keep relations - stay in bubble
00454                 acc_desired.angular.z *= scale_acc;
00455         }
00456 
00457         if(fabs(acc_desired.angular.z) > acc_max_rot_)
00458         {
00459                 scale_acc = fabs(acc_desired.angular.z) / acc_max_rot_;
00460                 acc_desired.angular.z *= scale_acc;
00461                 // again - keep relations - stay in bubble
00462                 acc_desired.linear.x *= scale_acc;
00463                 acc_desired.linear.y *= scale_acc;
00464         }
00465 
00466         // and get velocity-cmds by integrating them over one time-step
00467         last_vel_.linear.x = last_vel_.linear.x + acc_desired.linear.x / ctrl_freq_;
00468         last_vel_.linear.y = last_vel_.linear.y + acc_desired.linear.y / ctrl_freq_;
00469         last_vel_.angular.z = last_vel_.angular.z + acc_desired.angular.z / ctrl_freq_;
00470 
00471 
00472         // we are almost done now take into accoun stick-slip and similar nasty things
00473 
00474         // last checks - limit current twist cmd (upper and lower bounds)
00475         last_vel_ = limitTwist(last_vel_);
00476 
00477         // finally set robot_cmd (to non-zero value)
00478         robot_cmd = last_vel_;
00479 
00480         // now convert into robot-body frame
00481         robot_cmd = transformTwistFromFrame1ToFrame2(robot_cmd, ref_frame_band_, elastic_band_.at(0).center.pose);
00482 
00483         // check whether we reached the end of the band
00484         int curr_target_bubble = 1;
00485         while(fabs(bubble_diff.linear.x) <= tolerance_trans_ &&
00486                         fabs(bubble_diff.linear.y) <= tolerance_trans_ &&
00487                         fabs(bubble_diff.angular.z) <= tolerance_rot_)
00488         {
00489                 if(curr_target_bubble < ((int) elastic_band_.size()) - 1)
00490                 {
00491                   curr_target_bubble++;
00492                   // transform next target bubble into robot-body frame
00493                   // and get difference to robot bubble
00494                   bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose, elastic_band_.at(curr_target_bubble).center.pose,
00495                                                             ref_frame_band_);
00496                 }
00497                 else
00498                 {
00499                   ROS_DEBUG_THROTTLE_NAMED (1.0, "controller_state",
00500                                             "Goal reached with distance %.2f, %.2f, %.2f"
00501                                             "; sending zero velocity",
00502                                             bubble_diff.linear.x, bubble_diff.linear.y,
00503                                             bubble_diff.angular.z);
00504                   // goal position reached
00505                   robot_cmd.linear.x = 0.0;
00506                   robot_cmd.linear.y = 0.0;
00507                   robot_cmd.angular.z = 0.0;
00508                   // reset velocity
00509                   last_vel_.linear.x = 0.0;
00510                   last_vel_.linear.y = 0.0;
00511                   last_vel_.angular.z = 0.0;
00512                   break;
00513                 }
00514         }
00515 
00516         twist_cmd = robot_cmd;
00517         
00518         return true;
00519 }
00520 
00521 
00522 double EBandTrajectoryCtrl::getBubbleTargetVel(const int& target_bub_num, const std::vector<Bubble>& band, geometry_msgs::Twist& VelDir)
00523 {
00524         // init reference for direction vector
00525         VelDir.linear.x = 0.0;
00526         VelDir.linear.y = 0.0;
00527         VelDir.linear.z = 0.0;
00528         VelDir.angular.x = 0.0;
00529         VelDir.angular.y = 0.0;
00530         VelDir.angular.z = 0.0;
00531 
00532         // if we are looking at the last bubble - target vel is always zero
00533         if(target_bub_num >= ((int) band.size() - 1))
00534                 return 0.0;
00535 
00536 
00537         // otherwise check for max_vel calculated from current bubble size
00538         double v_max_curr_bub, v_max_next_bub;
00539         double bubble_distance, angle_to_pseudo_vel, delta_vel_max;
00540         geometry_msgs::Twist bubble_diff;
00541 
00542         // distance for braking s = 0.5*v*v/a
00543         v_max_curr_bub = sqrt(2 * elastic_band_.at(target_bub_num).expansion * acc_max_);
00544 
00545         // get distance to next bubble center
00546         ROS_ASSERT( (target_bub_num >= 0) && ((target_bub_num +1) < (int) band.size()) );
00547         bubble_diff = getFrame1ToFrame2InRefFrame(band.at(target_bub_num).center.pose, band.at(target_bub_num + 1).center.pose,
00548                                                                                                 ref_frame_band_);
00549         angle_to_pseudo_vel = bubble_diff.angular.z * costmap_ros_->getCircumscribedRadius();
00550 
00551         bubble_distance = sqrt( (bubble_diff.linear.x * bubble_diff.linear.x) + (bubble_diff.linear.y * bubble_diff.linear.y) +
00552                                                         (angle_to_pseudo_vel * angle_to_pseudo_vel) );
00553 
00554         // calculate direction vector - norm of diference
00555         VelDir.linear.x =bubble_diff.linear.x/bubble_distance;
00556         VelDir.linear.y =bubble_diff.linear.y/bubble_distance;
00557         VelDir.angular.z =bubble_diff.angular.z/bubble_distance;
00558 
00559         // if next bubble outside this one we will always be able to break fast enough
00560         if(bubble_distance > band.at(target_bub_num).expansion )
00561                 return v_max_curr_bub;
00562 
00563 
00564         // next bubble center inside this bubble - take into account restrictions on next bubble
00565         int next_bub_num = target_bub_num + 1;
00566         geometry_msgs::Twist dummy_twist;
00567         v_max_next_bub = getBubbleTargetVel(next_bub_num, band, dummy_twist); // recursive call
00568 
00569         // if velocity at next bubble bigger (or equal) than our velocity - we are on the safe side
00570         if(v_max_next_bub >= v_max_curr_bub)
00571                 return v_max_curr_bub;
00572 
00573 
00574         // otherwise max. allowed vel is next vel + plus possible reduction on the way between the bubble-centers
00575         delta_vel_max = sqrt(2 * bubble_distance * acc_max_);
00576         v_max_curr_bub = v_max_next_bub + delta_vel_max;
00577 
00578         return v_max_curr_bub;
00579 }
00580 
00581 
00582 geometry_msgs::Twist EBandTrajectoryCtrl::getFrame1ToFrame2InRefFrame(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame)
00583 {
00584 
00585         geometry_msgs::Pose2D frame1_pose2D, frame2_pose2D, ref_frame_pose2D;
00586         geometry_msgs::Pose2D frame1_pose2D_rf, frame2_pose2D_rf;
00587         geometry_msgs::Twist frame_diff;
00588 
00589         // transform all frames to Pose2d
00590         PoseToPose2D(frame1, frame1_pose2D);
00591         PoseToPose2D(frame2, frame2_pose2D);
00592         PoseToPose2D(ref_frame, ref_frame_pose2D);
00593 
00594         // transform frame1 into ref frame
00595         frame1_pose2D_rf.x = (frame1_pose2D.x - ref_frame_pose2D.x) * cos(ref_frame_pose2D.theta) +
00596                                                         (frame1_pose2D.y - ref_frame_pose2D.y) * sin(ref_frame_pose2D.theta);
00597         frame1_pose2D_rf.y = -(frame1_pose2D.x - ref_frame_pose2D.x) * sin(ref_frame_pose2D.theta) +
00598                                                         (frame1_pose2D.y - ref_frame_pose2D.y) * cos(ref_frame_pose2D.theta);
00599         frame1_pose2D_rf.theta = frame1_pose2D.theta - ref_frame_pose2D.theta;
00600         frame1_pose2D_rf.theta = angles::normalize_angle(frame1_pose2D_rf.theta);
00601         // transform frame2 into ref frame
00602         frame2_pose2D_rf.x = (frame2_pose2D.x - ref_frame_pose2D.x) * cos(ref_frame_pose2D.theta) +
00603                                                         (frame2_pose2D.y - ref_frame_pose2D.y) * sin(ref_frame_pose2D.theta);
00604         frame2_pose2D_rf.y = -(frame2_pose2D.x - ref_frame_pose2D.x) * sin(ref_frame_pose2D.theta) +
00605                                                         (frame2_pose2D.y - ref_frame_pose2D.y) * cos(ref_frame_pose2D.theta);
00606         frame2_pose2D_rf.theta = frame2_pose2D.theta - ref_frame_pose2D.theta;
00607         frame2_pose2D_rf.theta = angles::normalize_angle(frame2_pose2D_rf.theta);
00608 
00609         // get differences
00610         frame_diff.linear.x = frame2_pose2D_rf.x - frame1_pose2D_rf.x;
00611         frame_diff.linear.y = frame2_pose2D_rf.y - frame1_pose2D_rf.y;
00612         frame_diff.linear.z = 0.0;
00613         frame_diff.angular.x = 0.0;
00614         frame_diff.angular.y = 0.0;
00615         frame_diff.angular.z = frame2_pose2D_rf.theta - frame1_pose2D_rf.theta;
00616         // normalize angle
00617         frame_diff.angular.z = angles::normalize_angle(frame_diff.angular.z);
00618 
00619         return frame_diff;
00620 }
00621 
00622 
00623 geometry_msgs::Twist EBandTrajectoryCtrl::transformTwistFromFrame1ToFrame2(const geometry_msgs::Twist& curr_twist,
00624                                                                                                                         const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2)
00625 {
00626         geometry_msgs::Pose2D frame1_pose2D, frame2_pose2D;
00627         geometry_msgs::Twist tmp_transformed;
00628         double delta_ang;
00629 
00630         tmp_transformed = curr_twist;
00631 
00632         // transform all frames to Pose2d
00633         PoseToPose2D(frame1, frame1_pose2D);
00634         PoseToPose2D(frame2, frame2_pose2D);
00635 
00636         // get orientation diff of frames
00637         delta_ang = frame2_pose2D.theta - frame1_pose2D.theta;
00638         delta_ang = angles::normalize_angle(delta_ang);
00639 
00640         // tranform twist
00641         tmp_transformed.linear.x = curr_twist.linear.x * cos(delta_ang) + curr_twist.linear.y * sin(delta_ang);
00642         tmp_transformed.linear.y = -curr_twist.linear.x * sin(delta_ang) + curr_twist.linear.y * cos(delta_ang);
00643 
00644         return tmp_transformed;
00645 }
00646 
00647 
00648 geometry_msgs::Twist EBandTrajectoryCtrl::limitTwist(const geometry_msgs::Twist& twist)
00649 {
00650         geometry_msgs::Twist res = twist;
00651 
00652         //make sure to bound things by our velocity limits
00653         double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_;
00654         double lin_undershoot = min_vel_lin_ / sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y);
00655         if (lin_overshoot > 1.0) 
00656         {
00657                 res.linear.x /= lin_overshoot;
00658                 res.linear.y /= lin_overshoot;
00659                 // keep relations
00660                 res.angular.z /= lin_overshoot;
00661         }
00662 
00663         //we only want to enforce a minimum velocity if we're not rotating in place
00664         if(lin_undershoot > 1.0)
00665         {
00666                 res.linear.x *= lin_undershoot;
00667                 res.linear.y *= lin_undershoot;
00668                 // we cannot keep relations here for stability reasons
00669         }
00670 
00671         if (fabs(res.angular.z) > max_vel_th_)
00672         {
00673                 double scale = max_vel_th_/fabs(res.angular.z);
00674                 //res.angular.z = max_vel_th_ * sign(res.angular.z);
00675                 res.angular.z *= scale;
00676                 // keep relations
00677                 res.linear.x *= scale;
00678                 res.linear.y *= scale;
00679         }
00680 
00681         if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z);
00682         // we cannot keep relations here for stability reasons
00683 
00684         //we want to check for whether or not we're desired to rotate in place
00685         if(sqrt(twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y) < in_place_trans_vel_)
00686         {
00687                 if (fabs(res.angular.z) < min_in_place_vel_th_)
00688                         res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
00689 
00690                 res.linear.x = 0.0;
00691                 res.linear.y = 0.0;
00692         }
00693 
00694         ROS_DEBUG("Angular command %f", res.angular.z);
00695         return res;
00696 }
00697 
00698 
00699 }


eband_local_planner
Author(s): Christian Connette, Bhaskara Marthi
autogenerated on Fri Jan 3 2014 11:34:16