nav_pcontroller.cc
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Ingo Kresse <kresse@in.tum.de>, U. Klank
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 // For min/max
00032 #include <algorithm>
00033 
00034 #include <boost/bind.hpp>
00035 
00036 #include <math.h>
00037 #include <stdio.h>
00038 
00039 #include <geometry_msgs/Twist.h>
00040 
00041 #include "nav_pcontroller/nav_pcontroller.h"
00042 
00043 
00044 double BasePController::p_control(double x, double p, double limit)
00045 {
00046   return (x > 0) ? std::min(p*x, limit) : std::max(p*x, -limit);
00047 }
00048 
00049 
00050 double BasePController::limit_acc(double x, double x_old, double limit)
00051 {
00052   x = std::min(x, x_old + limit);
00053   x = std::max(x, x_old - limit);
00054   return x;
00055 }
00056 
00057 BasePController::BasePController()
00058   : n_("~"),
00059     move_base_actionserver_(n_, "move_base", false)
00060 {
00061   parseParams();
00062 
00063   move_base_actionserver_.registerGoalCallback(boost::bind(&BasePController::newMoveBaseGoal, this));
00064   move_base_actionserver_.registerPreemptCallback(boost::bind(&BasePController::preemptMoveBaseGoal, this));
00065   
00066   sub_goal_ = n_.subscribe("goal", 1, &BasePController::newGoal, this);
00067   pub_vel_ =  n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
00068 
00069   vx_=0; vy_=0; vth_=0;  // \todo: get from base driver
00070 
00071   goal_set_ = false;
00072 
00073   move_base_actionserver_.start();
00074 }
00075 
00076 BasePController::~BasePController()
00077 {
00078 }
00079 
00080 void BasePController::main()
00081 {
00082   ros::Rate loop(loop_rate_);
00083   ros::AsyncSpinner spinner(1);
00084 
00085   spinner.start();
00086   while(n_.ok()) {
00087     if(goal_set_)
00088       cycle();
00089     loop.sleep();
00090   }
00091 }
00092 
00093 void BasePController::parseParams()
00094 {
00095   n_.param<double>("xy_tolerance", xy_tolerance_, 0.005);
00096   n_.param<double>("th_tolerance", th_tolerance_, 0.005);
00097 
00098   double tmp_fail_timeout;
00099   n_.param<double>("fail_timeout", tmp_fail_timeout, 5.0);
00100   fail_timeout_ = ros::Duration(tmp_fail_timeout);
00101 
00102   n_.param<double>("fail_velocity", fail_velocity_, 0.02);
00103   n_.param<double>("vel_ang_max", vel_ang_max_, 0.2);
00104   n_.param<double>("vel_lin_max", vel_lin_max_, 0.2);
00105   n_.param<double>("acc_ang_max", acc_ang_max_, 0.4);
00106   n_.param<double>("acc_lin_max", acc_lin_max_, 0.4);
00107   n_.param<int>("loop_rate", loop_rate_, 30);
00108   n_.param<double>("p", p_, 1.2);
00109   n_.param<bool>("keep_distance", keep_distance_, true);
00110 
00111   n_.param<std::string>("global_frame", global_frame_, "map");
00112   n_.param<std::string>("base_link_frame", base_link_frame_, "base_link");
00113 
00114   //CHANGED
00115   //  dist_control_.setFootprint(0.42, -0.3075, 0.3075, -0.42, 0.0);
00116   /*(0.309, -0.43, 0.43, -0.309, 0.0);*/
00117   double front, rear, left, right, tolerance;
00118   std::string stName;
00119   n_.param("speed_filter_name", stName, std::string("/speed_filter"));
00120   n_.param(stName + "/footprint/left", left, 0.309);
00121   n_.param(stName + "/footprint/right", right, -0.309);
00122   n_.param(stName + "/footprint/front", front, 0.43);
00123   n_.param(stName + "/footprint/rear", rear, -0.43);
00124   n_.param(stName + "/footprint/tolerance", tolerance, 0.0);
00125 
00126   
00127   n_.param<double>("laser_watchdog_timeout", laser_watchdog_timeout_, 0.2);
00128 
00129   dist_control_.setFootprint(front, rear, left, right, tolerance);
00130 }
00131 
00132 void BasePController::newMoveBaseGoal()
00133 {
00134   move_base_msgs::MoveBaseGoal::ConstPtr msg = move_base_actionserver_.acceptNewGoal();
00135 
00136   // To be able to reconfigure the base controller on the fly, whe read the parameters whenever we receive a goal
00137   parseParams(); 
00138 
00139   newGoal(msg->target_pose);
00140   ROS_INFO("received goal: %f %f %f", x_goal_, y_goal_, th_goal_);
00141 }
00142 
00143 void BasePController::preemptMoveBaseGoal()
00144 {
00145   boost::mutex::scoped_lock curr_lock(lock);
00146 
00147   goal_set_ = false;
00148   stopRobot();
00149   move_base_actionserver_.setPreempted();
00150 }
00151 
00152 void BasePController::newGoal(const geometry_msgs::PoseStamped &msg)
00153 {
00154   boost::mutex::scoped_lock curr_lock(lock);
00155 
00156   geometry_msgs::PoseStamped goal;
00157 
00158   try
00159   {
00160     tf_.waitForTransform(
00161         global_frame_, msg.header.frame_id, msg.header.stamp, ros::Duration(1.0));
00162     tf_.transformPose(global_frame_, msg, goal);
00163   }
00164   catch(tf::TransformException& ex)
00165   {
00166     ROS_WARN("no localization information yet %s",ex.what());
00167     return;
00168   }
00169 
00170   x_goal_ = goal.pose.position.x;
00171   y_goal_ = goal.pose.position.y;
00172 
00173   // th = atan2(r21/2,r11/2)
00174   const geometry_msgs::Quaternion &q = goal.pose.orientation;
00175   th_goal_ = atan2(q.x*q.y + q.w*q.z, 0.5 - q.y*q.y -q.z*q.z);
00176 
00177   ROS_INFO("got goal: %f %f %f", x_goal_, y_goal_, th_goal_);
00178   
00179   low_speed_time_ = ros::Time::now();
00180 
00181   goal_set_ = true;
00182 }
00183 
00184 void BasePController::newGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
00185 {
00186   if(move_base_actionserver_.isActive())
00187     preemptMoveBaseGoal();
00188 
00189   newGoal(*msg);
00190 }
00191 
00193 bool BasePController::retrieve_pose()
00194 {
00195   tf::StampedTransform global_pose;
00196   try
00197   {
00198     tf_.lookupTransform(global_frame_, base_link_frame_, ros::Time(0), global_pose);
00199   }
00200   catch(tf::TransformException& ex)
00201   {
00202     ROS_WARN("no localization information yet %s",ex.what());
00203     return false;
00204   }
00205 
00206   // find out where we are now
00207   x_now_ = global_pose.getOrigin().x();
00208   y_now_ = global_pose.getOrigin().y();
00209 
00210   // th = atan2(r_21/2, r_11/2)
00211   const tf::Quaternion &q = global_pose.getRotation();
00212   th_now_ = atan2(q.x()*q.y() + q.w()*q.z(), 0.5 - q.y()*q.y() - q.z()*q.z());
00213 
00214   return true;
00215 }
00216 
00217 void BasePController::compute_p_control()
00218 {
00219   //compute differences (world space)
00220   double x_diff = x_goal_ - x_now_;
00221   double y_diff = y_goal_ - y_now_;
00222 
00223   // \todo: clean this ugly code
00224   double th_diff = fmod(th_goal_ - th_now_, 2*M_PI);
00225   if(th_diff > M_PI) th_diff = th_diff - 2*M_PI;
00226   if(th_diff < -M_PI) th_diff = th_diff + 2*M_PI;
00227 
00228   // transform to robot space
00229   double dx =  x_diff*cos(th_now_) + y_diff*sin(th_now_);
00230   double dy = -x_diff*sin(th_now_) + y_diff*cos(th_now_);
00231   double dth = th_diff;
00232 
00233   // do p-controller with limit (robot space)
00234   double vel_x =  p_control(dx, p_, vel_lin_max_);
00235   double vel_y =  p_control(dy, p_, vel_lin_max_);
00236   double vel_th = p_control(dth, p_, vel_ang_max_);
00237 
00238   // limit acceleration (robot space)
00239   vel_x  = limit_acc(vel_x, vx_, acc_lin_max_/loop_rate_);
00240   vel_y  = limit_acc(vel_y, vy_, acc_lin_max_/loop_rate_);
00241   vel_th = limit_acc(vel_th, vth_, acc_ang_max_/loop_rate_);
00242 
00243   // store resulting velocity
00244   vx_  = vel_x;
00245   vy_  = vel_y;
00246   vth_ = vel_th;
00247 }
00248 
00249 #ifndef max
00250 #define max(A,B) (A) > (B) ? (A) : (B)
00251 #endif
00252 void BasePController::cycle()
00253 {
00254   boost::mutex::scoped_lock curr_lock(lock);
00255 
00256   if(!retrieve_pose()) {
00257     stopRobot();
00258     return;
00259   }
00260 
00261   if ( !dist_control_.fresh_scans(laser_watchdog_timeout_) ) {
00262     stopRobot();
00263     return;
00264   }
00265 
00266   compute_p_control();
00267   
00268   if (keep_distance_)
00269     dist_control_.compute_distance_keeping(&vx_, &vy_, &vth_);
00270 
00271   sendVelCmd(vx_, vy_, vth_);
00272 
00273   if(comparePoses(x_goal_, y_goal_, th_goal_, x_now_, y_now_, th_now_)) {
00274     
00275     if(move_base_actionserver_.isActive())
00276       move_base_actionserver_.setSucceeded();
00277     goal_set_ = false;
00278     stopRobot();
00279   }
00280   // Sort of a bad hack. It might be a bad idea to 'unify' angular
00281   // and linear velocity and just take te maximum.
00282   // double velocity = max(sqrt(vx_ * vx_+ vy_ * vy_) , abs(vth_));
00283   double velocity = sqrt(vx_ * vx_+ vy_ * vy_);
00284 
00285   if( velocity < fail_velocity_ )
00286   {
00287     if( ros::Time::now() - low_speed_time_ > fail_timeout_ )
00288     {
00289       goal_set_ = false;
00290       stopRobot();
00291       
00292       if(move_base_actionserver_.isActive())
00293         move_base_actionserver_.setAborted();
00294       return;
00295     }
00296   }
00297   else
00298     low_speed_time_ = ros::Time::now();
00299 
00300   if(move_base_actionserver_.isActive())
00301   {
00302     move_base_msgs::MoveBaseFeedback feedback;
00303     feedback.base_position.header.stamp = ros::Time::now();
00304     feedback.base_position.header.frame_id = global_frame_;
00305     feedback.base_position.pose.position.x = x_now_;
00306     feedback.base_position.pose.position.y = y_now_;
00307     feedback.base_position.pose.position.z = 0.0;
00308     feedback.base_position.pose.orientation = tf::createQuaternionMsgFromYaw(th_now_);
00309     move_base_actionserver_.publishFeedback(feedback);
00310   }
00311 }
00312 
00313 #define ANG_NORM(a) atan2(sin((a)),cos((a)))
00314 
00315 void BasePController::stopRobot()
00316 {
00317   sendVelCmd(0.0,0.0,0.0);
00318 }
00319 
00320 void BasePController::sendVelCmd(double vx, double vy, double vth)
00321 {
00322   geometry_msgs::Twist cmdvel;
00323 
00324   cmdvel.linear.x = vx;
00325   cmdvel.linear.y = vy;
00326   cmdvel.angular.z = vth;
00327 
00328   pub_vel_.publish(cmdvel);
00329 }
00330 
00331 
00332 bool BasePController::comparePoses(double x1, double y1, double a1,
00333                                    double x2, double y2, double a2)
00334 {
00335   bool res;
00336   if((fabs(x2-x1) <= xy_tolerance_) &&
00337      (fabs(y2-y1) <= xy_tolerance_) &&
00338      (fabs(ANG_NORM(ANG_NORM(a2)-ANG_NORM(a1))) <= th_tolerance_))
00339     res = true;
00340   else
00341     res = false;
00342   return(res);
00343 }
00344 
00345 int main(int argc, char *argv[])
00346 {
00347   ros::init(argc, argv, "nav_pcontroller");
00348 
00349   BasePController pc;
00350   pc.main();
00351   return 0;
00352 }


nav_pcontroller
Author(s): Ingo Kresse
autogenerated on Thu Jun 6 2019 20:28:18