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 
00060 #include <unistd.h>
00061 #include <math.h>
00062 #include <stdio.h>
00063 
00064 // For min/max
00065 #include <algorithm>
00066 
00067 #include <boost/thread.hpp>
00068 
00069 #include <ros/ros.h>
00070 #include <ros/time.h>
00071 #include "tf/transform_listener.h"
00072 
00073 
00074 // The messages that we'll use
00075 #include <geometry_msgs/PoseStamped.h>
00076 #include <geometry_msgs/Twist.h>
00077 #include <std_msgs/String.h>
00078 #include <sensor_msgs/LaserScan.h>
00079 #include <move_base_msgs/MoveBaseAction.h>
00080 #include <actionlib/server/simple_action_server.h>
00081 
00082 #ifdef JLO_BASE_CONTROL
00083 #include <std_msgs/UInt64.h>
00084 #include <vision_srvs/srvjlo.h>
00085 using namespace vision_srvs;
00086 #define JLO_IDQUERY "idquery"
00087 #define JLO_FRAMEQUERY "framequery"
00088 #define JLO_NAMEQUERY "namequery"
00089 #define JLO_DELETE "del"
00090 #define JLO_UPDATE "update"
00091 
00092 #include <navp_action/nav_actionAction.h>
00093 #endif
00094 
00095 #include "BaseDistance.h"
00096 
00097 class BasePController {
00098 private:
00099 
00100   double xy_tolerance_, th_tolerance_;
00101   ros::Duration fail_timeout_;
00102   double fail_velocity_;
00103   double vel_ang_max_, vel_lin_max_, acc_ang_max_, acc_lin_max_, p_;
00104   int loop_rate_;
00105 
00106   double vx_, vy_, vth_;
00107   double x_goal_, y_goal_, th_goal_;
00108   double x_now_, y_now_, th_now_;
00109   bool goal_set_, keep_distance_;
00110 
00111   std::string global_frame_;
00112   std::string base_link_frame_;
00113 
00114   BaseDistance dist_control_;
00115 
00116   ros::NodeHandle n_;
00117   tf::TransformListener tf_;
00118   ros::Subscriber sub_goal_;
00119   ros::Publisher pub_vel_;
00120 
00121   ros::Time low_speed_time_;
00122 
00123   boost::mutex lock;
00124 
00125   void newGoal(const geometry_msgs::PoseStamped &msg);
00126   void newGoal(const geometry_msgs::PoseStamped::ConstPtr& msg);
00127 
00128   //  void laserData(const sensor_msgs::LaserScan::ConstPtr& msg);
00129 
00130   void cycle();
00131   void stopRobot();
00132   void sendVelCmd(double vx, double vy, double vth);
00133   bool comparePoses(double x1, double y1, double a1,
00134                     double x2, double y2, double a2);
00135 
00136   bool retrieve_pose();
00137   double p_control(double x, double p, double limit);
00138   double limit_acc(double x, double x_old, double limit);
00139   void compute_p_control();
00140 
00141   void parseParams();
00142 
00143 #ifdef JLO_BASE_CONTROL
00144   bool jlo_enabled_;
00145   unsigned long map_jlo_id_;
00146   ros::ServiceClient client_;
00147   actionlib::SimpleActionServer<navp_action::nav_actionAction> *jlo_actionserver_;
00148 
00149   unsigned long namequery_jlo(std::string name);
00150   bool query_jlo(unsigned long id, double &x, double &y, double &theta);
00151 
00152   void newJloActionGoal();
00153   void preemptJloActionGoal();
00154 #endif
00155 
00156   actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> move_base_actionserver_;
00157 
00158   void newMoveBaseGoal();
00159   void preemptMoveBaseGoal();
00160 
00161 public:
00162   BasePController();
00163   ~BasePController();
00164   void main();
00165 };
00166 
00167 double BasePController::p_control(double x, double p, double limit)
00168 {
00169   return (x > 0) ? std::min(p*x, limit) : std::max(p*x, -limit);
00170 }
00171 
00172 
00173 double BasePController::limit_acc(double x, double x_old, double limit)
00174 {
00175   x = std::min(x, x_old + limit);
00176   x = std::max(x, x_old - limit);
00177   return x;
00178 }
00179 
00180 /*
00181 void BasePController::laserData(const sensor_msgs::LaserScan::ConstPtr& msg)
00182 {
00183   dist_control_.laser_collect(msg);
00184 }
00185 */
00186 
00187 BasePController::BasePController()
00188   : n_("~"),
00189     move_base_actionserver_(n_, "move_base")
00190 {
00191   parseParams();
00192 
00193   move_base_actionserver_.registerGoalCallback(boost::bind(&BasePController::newMoveBaseGoal, this));
00194   move_base_actionserver_.registerPreemptCallback(boost::bind(&BasePController::preemptMoveBaseGoal, this));
00195   
00196 #ifdef JLO_BASE_CONTROL
00197   if(jlo_enabled_)
00198   {
00199     jlo_actionserver_ = new actionlib::SimpleActionServer<navp_action::nav_actionAction>(n_, "nav_action");
00200 
00201     jlo_actionserver_->registerGoalCallback(boost::bind(&BasePController::newJloActionGoal, this));
00202     jlo_actionserver_->registerPreemptCallback(boost::bind(&BasePController::preemptJloActionGoal, this));
00203 
00204     ros::service::waitForService("/located_object");
00205     client_ = n_.serviceClient<srvjlo>("/located_object", true);
00206     map_jlo_id_ = namequery_jlo(global_frame_);
00207   }
00208   else
00209     jlo_actionserver_ = 0;
00210 #endif
00211 
00212   sub_goal_ = n_.subscribe("/goal", 1, &BasePController::newGoal, this);
00213   pub_vel_ =  n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
00214 
00215   vx_=0; vy_=0; vth_=0;  // \todo: get from base driver
00216 
00217   goal_set_ = false;
00218 }
00219 
00220 BasePController::~BasePController()
00221 {
00222   #ifdef JLO_BASE_CONTROL
00223   delete jlo_actionserver_;
00224   #endif
00225 }
00226 
00227 void BasePController::main()
00228 {
00229   ros::Rate loop(loop_rate_);
00230   ros::AsyncSpinner spinner(1);
00231 
00232   spinner.start();
00233   while(n_.ok()) {
00234     if(goal_set_)
00235       cycle();
00236     loop.sleep();
00237   }
00238 }
00239 
00240 void BasePController::parseParams()
00241 {
00242   n_.param<double>("xy_tolerance", xy_tolerance_, 0.005);
00243   n_.param<double>("th_tolerance", th_tolerance_, 0.005);
00244 
00245   double tmp_fail_timeout;
00246   n_.param<double>("fail_timeout", tmp_fail_timeout, 5.0);
00247   fail_timeout_ = ros::Duration(tmp_fail_timeout);
00248 
00249   n_.param<double>("fail_velocity", fail_velocity_, 0.02);
00250   n_.param<double>("vel_ang_max", vel_ang_max_, 0.2);
00251   n_.param<double>("vel_lin_max", vel_lin_max_, 0.2);
00252   n_.param<double>("acc_ang_max", acc_ang_max_, 0.4);
00253   n_.param<double>("acc_lin_max", acc_lin_max_, 0.4);
00254   n_.param<int>("loop_rate", loop_rate_, 30);
00255   n_.param<double>("p", p_, 1.2);
00256   n_.param<bool>("keep_distance", keep_distance_, true);
00257 
00258 #ifdef JLO_BASE_CONTROL
00259   n_.param<bool>("enable_jlo", jlo_enabled_, true);
00260 #endif
00261 
00262   n_.param<std::string>("global_frame", global_frame_, "/map");
00263   n_.param<std::string>("base_link_frame", base_link_frame_, "/base_link");
00264 
00265   //CHANGED
00266   //  dist_control_.setFootprint(0.42, -0.3075, 0.3075, -0.42, 0.0);
00267   /*(0.309, -0.43, 0.43, -0.309, 0.0);*/
00268   double front, rear, left, right, tolerance;
00269   std::string stName;
00270   n_.param("speed_filter_name", stName, std::string("/speed_filter"));
00271   n_.param(stName + "/footprint/left", left, 0.309);
00272   n_.param(stName + "/footprint/right", right, -0.309);
00273   n_.param(stName + "/footprint/front", front, 0.43);
00274   n_.param(stName + "/footprint/rear", rear, -0.43);
00275   n_.param(stName + "/footprint/tolerance", tolerance, 0.0);
00276   
00277   dist_control_.setFootprint(front, rear, left, right, tolerance);
00278 }
00279 
00280 #ifdef JLO_BASE_CONTROL
00281 unsigned long BasePController::namequery_jlo(std::string name)
00282 {
00283   srvjlo msg;
00284   msg.request.query.name = name;
00285   msg.request.command = JLO_NAMEQUERY;
00286 
00287   if(!client_.isValid())
00288   {
00289     ros::service::waitForService("/located_object");
00290     client_ = n_.serviceClient<srvjlo>("/located_object", true);
00291   }
00292   if (!client_.call(msg))
00293   {
00294     printf("Error asking jlo namequery %s!\n", name.c_str());
00295     return 1;
00296   }
00297   else if (msg.response.error.length() > 0)
00298   {
00299     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00300     return 1;
00301   }
00302   return msg.response.answer.id;
00303 }
00304 
00305 bool BasePController::query_jlo(unsigned long id, double &x, double &y, double &theta)
00306 {
00307   srvjlo msg;
00308   msg.request.query.id = id;
00309   msg.request.query.parent_id = map_jlo_id_;
00310   msg.request.command = JLO_FRAMEQUERY;
00311   if(id == 1)
00312      msg.request.command = JLO_IDQUERY;
00313 
00314   if(!client_.isValid())
00315   {
00316     ros::service::waitForService("/located_object");
00317     client_ = n_.serviceClient<srvjlo>("/located_object", true);
00318   }
00319   if (!client_.call(msg))
00320   {
00321 
00322     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00323     return false;
00324   }
00325   else if (msg.response.error.length() > 0)
00326   {
00327     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00328     return false;
00329   }
00330   vision_msgs::partial_lo::_pose_type &vectmp = msg.response.answer.pose;
00331   x = vectmp[3];
00332   y = vectmp[7];
00333   printf("Rotationmatrix\n%f %f %f\n %f %f %f\n %f %f %f\n", vectmp[0],
00334   vectmp[1],vectmp[2],
00335   vectmp[4],vectmp[5],vectmp[6],
00336   vectmp[8],vectmp[9],vectmp[10]);
00337 
00338   theta = atan2(vectmp[4], vectmp[0]);
00339 
00340   printf("=> %f theta\n", theta);
00341 
00342   return true;
00343 }
00344 
00345 void BasePController::newJloActionGoal()
00346 {
00347   if(move_base_actionserver_.isActive())
00348     preemptMoveBaseGoal();
00349       
00350   navp_action::nav_actionGoal::ConstPtr msg = jlo_actionserver_->acceptNewGoal();
00351 
00352   // To be able to reconfigure the base controller on the fly, whe read the parameters whenever we receive a goal
00353   parseParams();
00354 
00355   // Jlo might block for quite some time, so we do not want to hold
00356   // the lock while calling jlo. Rather, we first store the goal in
00357   // temporary variables and copy after the jlo call.
00358   double x_goal, y_goal, th_goal;
00359   if(query_jlo(msg->target_lo.data, x_goal, y_goal, th_goal))
00360   {
00361     boost::mutex::scoped_lock curr_lock(lock);
00362 
00363     x_goal_ = x_goal;
00364     y_goal_ = y_goal;
00365     th_goal_ = th_goal;
00366 
00367     low_speed_time_ = ros::Time::now();
00368 
00369     ROS_INFO("received goal: %f %f %f", x_goal_, y_goal_, th_goal_);
00370     goal_set_ = true;
00371   }
00372   else
00373   {
00374     ROS_WARN("Jlo query for goal %ld failed.", msg->target_lo.data);
00375     jlo_actionserver_->setAborted();
00376   }
00377 }
00378 
00379 void BasePController::preemptJloActionGoal()
00380 {
00381   boost::mutex::scoped_lock curr_lock(lock);
00382 
00383   goal_set_ = false;
00384   stopRobot();
00385   jlo_actionserver_->setPreempted();
00386 }
00387 #endif
00388 
00389 void BasePController::newMoveBaseGoal()
00390 {
00391   #ifdef JLO_BASE_CONTROL
00392   if(jlo_actionserver_ && jlo_actionserver_->isActive())
00393     preemptJloActionGoal();
00394   #endif
00395 
00396   move_base_msgs::MoveBaseGoal::ConstPtr msg = move_base_actionserver_.acceptNewGoal();
00397 
00398   // To be able to reconfigure the base controller on the fly, whe read the parameters whenever we receive a goal
00399   //parseParams();
00400 
00401   newGoal(msg->target_pose);
00402   ROS_INFO("received goal: %f %f %f", x_goal_, y_goal_, th_goal_);
00403 }
00404 
00405 void BasePController::preemptMoveBaseGoal()
00406 {
00407   boost::mutex::scoped_lock curr_lock(lock);
00408 
00409   goal_set_ = false;
00410   stopRobot();
00411   move_base_actionserver_.setPreempted();
00412 }
00413 
00414 void BasePController::newGoal(const geometry_msgs::PoseStamped &msg)
00415 {
00416   boost::mutex::scoped_lock curr_lock(lock);
00417 
00418   geometry_msgs::PoseStamped goal;
00419 
00420 
00421   try
00422   {
00423     tf_.transformPose(global_frame_, msg, goal);
00424   }
00425   catch(tf::TransformException& ex)
00426   {
00427     ROS_WARN("no localization information yet %s",ex.what());
00428     return;
00429   }
00430 
00431   x_goal_ = goal.pose.position.x;
00432   y_goal_ = goal.pose.position.y;
00433 
00434   // th = atan2(r21/2,r11/2)
00435   const geometry_msgs::Quaternion &q = goal.pose.orientation;
00436   th_goal_ = atan2(q.x*q.y + q.w*q.z, 0.5 - q.y*q.y -q.z*q.z);
00437 
00438   ROS_INFO("got goal: %f %f %f", x_goal_, y_goal_, th_goal_);
00439   
00440   low_speed_time_ = ros::Time::now();
00441 
00442   goal_set_ = true;
00443 }
00444 
00445 void BasePController::newGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
00446 {
00447 #ifdef JLO_BASE_CONTROL
00448   if(jlo_actionserver_ && jlo_actionserver_->isActive())
00449     preemptJloActionGoal();
00450 #endif
00451   if(move_base_actionserver_.isActive())
00452     preemptMoveBaseGoal();
00453 
00454   newGoal(*msg);
00455 }
00456 
00458 bool BasePController::retrieve_pose()
00459 {
00460   tf::StampedTransform global_pose;
00461   try
00462   {
00463     tf_.lookupTransform(global_frame_, base_link_frame_, ros::Time(0), global_pose);
00464   }
00465   catch(tf::TransformException& ex)
00466   {
00467     ROS_WARN("no localization information yet %s",ex.what());
00468     return false;
00469   }
00470 
00471   // find out where we are now
00472   x_now_ = global_pose.getOrigin().x();
00473   y_now_ = global_pose.getOrigin().y();
00474 
00475   // th = atan2(r_21/2, r_11/2)
00476   const btQuaternion &q = global_pose.getRotation();
00477   th_now_ = atan2(q.x()*q.y() + q.w()*q.z(), 0.5 - q.y()*q.y() - q.z()*q.z());
00478 
00479   return true;
00480 }
00481 
00482 void BasePController::compute_p_control()
00483 {
00484   //compute differences (world space)
00485   double x_diff = x_goal_ - x_now_;
00486   double y_diff = y_goal_ - y_now_;
00487 
00488   // \todo: clean this ugly code
00489   double th_diff = fmod(th_goal_ - th_now_, 2*M_PI);
00490   if(th_diff > M_PI) th_diff = th_diff - 2*M_PI;
00491   if(th_diff < -M_PI) th_diff = th_diff + 2*M_PI;
00492 
00493   // transform to robot space
00494   double dx =  x_diff*cos(th_now_) + y_diff*sin(th_now_);
00495   double dy = -x_diff*sin(th_now_) + y_diff*cos(th_now_);
00496   double dth = th_diff;
00497 
00498   // do p-controller with limit (robot space)
00499   double vel_x =  p_control(dx, p_, vel_lin_max_);
00500   double vel_y =  p_control(dy, p_, vel_lin_max_);
00501   double vel_th = p_control(dth, p_, vel_ang_max_);
00502 
00503   // limit acceleration (robot space)
00504   vel_x  = limit_acc(vel_x, vx_, acc_lin_max_/loop_rate_);
00505   vel_y  = limit_acc(vel_y, vy_, acc_lin_max_/loop_rate_);
00506   vel_th = limit_acc(vel_th, vth_, acc_ang_max_/loop_rate_);
00507 
00508   // store resulting velocity
00509   vx_  = vel_x;
00510   vy_  = vel_y;
00511   vth_ = vel_th;
00512 }
00513 
00514 #ifndef max
00515 #define max(A,B) (A) > (B) ? (A) : (B)
00516 #endif
00517 void BasePController::cycle()
00518 {
00519   boost::mutex::scoped_lock curr_lock(lock);
00520 
00521   if(!retrieve_pose()) {
00522     stopRobot();
00523     return;
00524   }
00525 
00526   compute_p_control();
00527   
00528   if (keep_distance_)
00529     dist_control_.compute_distance_keeping(&vx_, &vy_, &vth_);
00530 
00531   sendVelCmd(vx_, vy_, vth_);
00532 
00533   if(comparePoses(x_goal_, y_goal_, th_goal_, x_now_, y_now_, th_now_)) {
00534 #ifdef JLO_BASE_CONTROL
00535     if(jlo_actionserver_ && jlo_actionserver_->isActive())
00536     {
00537       navp_action::nav_actionResult result;
00538       result.distance.data = sqrt((x_goal_-x_now_)*(x_goal_-x_now_) + (y_goal_-y_now_)*(y_goal_-y_now_));
00539       ROS_INFO("nav_pcontroller: Reached goal %f (%f rot reached %f commanded)\n",
00540         result.distance.data, th_now_, th_goal_);
00541       jlo_actionserver_->setSucceeded(result);
00542     }
00543 #endif
00544     if(move_base_actionserver_.isActive())
00545       move_base_actionserver_.setSucceeded();
00546     goal_set_ = false;
00547     stopRobot();
00548   }
00549   // Sort of a bad hack. It might be a bad idea to 'unify' angular
00550   // and linear velocity and just take te maximum.
00551   double velocity = max(sqrt(vx_ * vx_+ vy_ * vy_) , vth_);
00552 
00553   if( velocity < fail_velocity_ )
00554   {
00555     if( ros::Time::now() - low_speed_time_ > fail_timeout_ )
00556     {
00557       goal_set_ = false;
00558       stopRobot();
00559 #ifdef JLO_BASE_CONTROL
00560       if(jlo_actionserver_ && jlo_actionserver_->isActive())
00561       {
00562         navp_action::nav_actionResult result;
00563         result.distance.data = sqrt((x_goal_-x_now_)*(x_goal_-x_now_) + (y_goal_-y_now_)*(y_goal_-y_now_));
00564         jlo_actionserver_->setAborted(result);
00565       }
00566 #endif
00567       if(move_base_actionserver_.isActive())
00568         move_base_actionserver_.setAborted();
00569       return;
00570     }
00571   }
00572   else
00573     low_speed_time_ = ros::Time::now();
00574 
00575 #ifdef JLO_BASE_CONTROL
00576   if(jlo_actionserver_ && jlo_actionserver_->isActive())
00577   {
00578     navp_action::nav_actionFeedback feedback;
00579     feedback.speed.data = velocity;
00580     feedback.distance.data = sqrt((x_goal_-x_now_)*(x_goal_-x_now_) + (y_goal_-y_now_)*(y_goal_-y_now_));
00581     jlo_actionserver_->publishFeedback(feedback);
00582   }
00583 #endif
00584   if(move_base_actionserver_.isActive())
00585   {
00586     move_base_msgs::MoveBaseFeedback feedback;
00587     feedback.base_position.header.stamp = ros::Time::now();
00588     feedback.base_position.header.frame_id = global_frame_;
00589     feedback.base_position.pose.position.x = x_now_;
00590     feedback.base_position.pose.position.y = y_now_;
00591     feedback.base_position.pose.position.z = 0.0;
00592     feedback.base_position.pose.orientation = tf::createQuaternionMsgFromYaw(th_now_);
00593     move_base_actionserver_.publishFeedback(feedback);
00594   }
00595 }
00596 
00597 #define ANG_NORM(a) atan2(sin((a)),cos((a)))
00598 
00599 void BasePController::stopRobot()
00600 {
00601   sendVelCmd(0.0,0.0,0.0);
00602 }
00603 
00604 void BasePController::sendVelCmd(double vx, double vy, double vth)
00605 {
00606   geometry_msgs::Twist cmdvel;
00607 
00608   cmdvel.linear.x = vx;
00609   cmdvel.linear.y = vy;
00610   cmdvel.angular.z = vth;
00611 
00612   pub_vel_.publish(cmdvel);
00613 }
00614 
00615 
00616 bool BasePController::comparePoses(double x1, double y1, double a1,
00617                                    double x2, double y2, double a2)
00618 {
00619   bool res;
00620   if((fabs(x2-x1) <= xy_tolerance_) &&
00621      (fabs(y2-y1) <= xy_tolerance_) &&
00622      (fabs(ANG_NORM(ANG_NORM(a2)-ANG_NORM(a1))) <= th_tolerance_))
00623     res = true;
00624   else
00625     res = false;
00626   return(res);
00627 }
00628 
00629 int main(int argc, char *argv[])
00630 {
00631   ros::init(argc, argv, "nav_pcontroller");
00632 
00633   BasePController pc;
00634   pc.main();
00635   return 0;
00636 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


nav_pcontroller
Author(s): Ingo Kresse
autogenerated on Thu May 23 2013 09:13:24