cob_linear_nav.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016  
00017 
00018 //##################
00019 //#### includes ####
00020 
00021 // standard includes
00022 //--
00023 
00024 // ROS includes
00025 #include <ros/ros.h>
00026 #include <angles/angles.h>
00027 
00028 #include <pthread.h>
00029 
00030 // ROS message includes
00031 #include <actionlib/client/simple_action_client.h>
00032 #include <actionlib/server/simple_action_server.h>
00033 #include <move_base_msgs/MoveBaseAction.h>
00034 
00035 #include <geometry_msgs/Twist.h>
00036 #include <geometry_msgs/PoseStamped.h>
00037 #include <nav_msgs/Odometry.h>
00038 #include <cob_srvs/SetString.h>
00039 
00040 #include <tf/transform_listener.h>
00041 
00042 //####################
00043 //#### node class ####
00044 class NodeClass
00045 {
00046   public:
00047   // create a handle for this node, initialize node
00048   ros::NodeHandle nh_;
00049 
00050   // declaration of topics
00051   ros::Publisher topic_pub_command_;
00052 
00053   ros::Subscriber goal_sub_, odometry_sub_;
00054 
00055   ros::ServiceServer ss_;
00056 
00057   // declaration of action server
00058   actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> as_;
00059   move_base_msgs::MoveBaseResult result_; //result is of type geometry_msgs/PoseStamped
00060 
00061   //declaration of action client to forward move_base_simple messages
00062   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> * action_client_;
00063 
00064 
00065   // Constructor
00066   NodeClass(std::string name) :
00067     as_(nh_, name, boost::bind(&NodeClass::actionCB, this, _1), false)
00068   {
00069     m_mutex = PTHREAD_MUTEX_INITIALIZER;
00070 
00071     ros::NodeHandle private_nh("~");
00072 
00073     // implementation of topics to publish
00074     topic_pub_command_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00075 
00076     //we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
00077     //they won't get any useful information back about its status, but this is useful for tools
00078     //like nav_view and rviz
00079     ros::NodeHandle simple_nh("move_base_linear_simple");
00080     goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&NodeClass::topicCB, this, _1));
00081 
00082     action_client_ = new actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>(nh_, name);
00083 
00084     // subscribe to odometry
00085     odometry_sub_ = nh_.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&NodeClass::odometryCB, this, _1));
00086 
00087     ss_ = private_nh.advertiseService("set_global_frame", &NodeClass::serviceCB, this);
00088 
00089     // read parameters from parameter server
00090     if(!private_nh.hasParam("kv")) ROS_WARN("Used default parameter for kv [2.5]");
00091     private_nh.param("kv", kv_, 2.5);
00092 
00093     if(!private_nh.hasParam("kp")) ROS_WARN("Used default parameter for kp [3.0]");
00094     private_nh.param("kp", kp_, 3.0);
00095 
00096     if(!private_nh.hasParam("virt_mass")) ROS_WARN("Used default parameter for virt_mass [0.5]");
00097     private_nh.param("virt_mass", virt_mass_, 0.5);
00098 
00099     if(!private_nh.hasParam("vmax")) ROS_WARN("Used default parameter for vmax [0.3 m/s]");
00100     private_nh.param("vmax", v_max_, 0.3);
00101 
00102     if(!private_nh.hasParam("goal_threshold")) ROS_WARN("Used default parameter for goal_threshold [0.03 cm]");
00103     private_nh.param("goal_threshold", goal_threshold_, 0.03);
00104 
00105     if(!private_nh.hasParam("speed_threshold")) ROS_WARN("Used default parameter for speed_threshold [0.08 m/s]");
00106     private_nh.param("speed_threshold", speed_threshold_, 0.08);
00107 
00108     if(!private_nh.hasParam("kv_rot")) ROS_WARN("Used default parameter for kv_rot [2.0]");
00109     private_nh.param("kv_rot", kv_rot_, 2.0);
00110 
00111     if(!private_nh.hasParam("kp_rot")) ROS_WARN("Used default parameter for kp_rot [2.0]");
00112     private_nh.param("kp_rot", kp_rot_, 2.0);
00113 
00114     if(!private_nh.hasParam("virt_mass_rot")) ROS_WARN("Used default parameter for virt_mass_rot [0.5]");
00115     private_nh.param("virt_mass_rot", virt_mass_rot_, 0.5);
00116 
00117     if(!private_nh.hasParam("vtheta_max")) ROS_WARN("Used default parameter for vtheta_max [0.3 rad/s]");
00118     private_nh.param("vtheta_max", vtheta_max_, 0.3);
00119 
00120     if(!private_nh.hasParam("goal_threshold_rot")) ROS_WARN("Used default parameter for goal_threshold_rot [0.08 rad]");
00121     private_nh.param("goal_threshold_rot", goal_threshold_rot_, 0.08);
00122 
00123     if(!private_nh.hasParam("speed_threshold_rot")) ROS_WARN("Used default parameter for speed_threshold_rot [0.08 rad/s]");
00124     private_nh.param("speed_threshold_rot", speed_threshold_rot_, 0.08);
00125 
00126     if(!private_nh.hasParam("global_frame")) ROS_WARN("Used default parameter for global_frame [/map]");
00127     private_nh.param("global_frame", global_frame_, std::string("map"));
00128 
00129     if(!private_nh.hasParam("robot_frame")) ROS_WARN("Used default parameter for robot_frame [/base_link]");
00130     private_nh.param("robot_frame", robot_frame_, std::string("base_link"));
00131 
00132     if(!private_nh.hasParam("slow_down_distance")) ROS_WARN("Used default parameter for slow_down_distance [0.5m]");
00133     private_nh.param("slow_down_distance", slow_down_distance_, 0.5);
00134 
00135     if(!private_nh.hasParam("goal_abortion_time")) ROS_WARN("Used default parameter for goal_abortion_time [5.0s]");
00136     private_nh.param("goal_abortion_time", goal_abortion_time_, 5.0);
00137 
00138     if(!private_nh.hasParam("use_move_action")) ROS_WARN("Used default parameter for use_move_action [true]");
00139     private_nh.param("use_move_action", use_move_action_, true);
00140 
00141     //generate robot zero_pose
00142     zero_pose_.pose.position.x = 0.0;
00143     zero_pose_.pose.position.y = 0.0;
00144     zero_pose_.pose.position.z = 0.0;
00145     zero_pose_.pose.orientation.x = 0.0;
00146     zero_pose_.pose.orientation.y = 0.0;
00147     zero_pose_.pose.orientation.z = 0.0;
00148     zero_pose_.pose.orientation.w = 1.0;
00149     zero_pose_.header.frame_id = robot_frame_;
00150     zero_pose_.header.stamp = ros::Time::now();
00151     robot_pose_global_ = zero_pose_;
00152     robot_pose_global_.header.frame_id = global_frame_;
00153     goal_pose_global_ = robot_pose_global_;
00154     // at startup, the robot is should not move
00155     move_ = false;
00156 
00157     // also initialize variables that are used later on!
00158     last_time_ = ros::Time::now().toSec();
00159     vtheta_last_ = 0.0;
00160 
00161     //start action server, it holds the main loop while driving
00162     as_.start();
00163   }
00164 
00165   void topicCB(const geometry_msgs::PoseStamped::ConstPtr& goal)
00166   {
00167 
00168     if(!goalValid(*goal))
00169       return;
00170 
00171     if(use_move_action_)
00172     {
00173       ROS_INFO("In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
00174       move_base_msgs::MoveBaseGoal action_goal;
00175 
00176       action_goal.target_pose = transformGoalToMap(*goal);
00177 
00178       action_client_->sendGoal(action_goal);
00179       action_client_->stopTrackingGoal();
00180     }
00181     else
00182     {
00183       ROS_DEBUG("In ROS goal callback, using the PoseStamped as error and start control step.");
00184 
00185       last_time_moving_ = ros::Time::now().toSec();
00186 
00187       getRobotPoseGlobal();
00188 
00189       if(last_time_ < 0)
00190       {
00191         vtheta_last_ = 0.0f;
00192         vx_last_ = 0.0f;
00193         vy_last_ = 0.0f;
00194         last_time_ = ros::Time::now().toSec();
00195       }
00196 
00197       x_last_ = robot_pose_global_.pose.position.x;
00198       y_last_ = robot_pose_global_.pose.position.y;
00199       theta_last_ = tf::getYaw(robot_pose_global_.pose.orientation);
00200 
00201       goal_pose_global_ = transformGoalToMap(*goal);
00202 
00203       move_ = true;
00204 
00205     }
00206 
00207   }
00208 
00209   void actionCB(const move_base_msgs::MoveBaseGoalConstPtr &goal)
00210   {
00211     if(!goalValid(goal->target_pose))
00212     {
00213       as_.setAborted(move_base_msgs::MoveBaseResult(), "Aborting because a transformation could not be found");
00214       return;
00215     }
00216     // goal is of type geometry_msgs/PoseStamped
00217     ROS_INFO("In idle mode, new goal accepted");
00218 
00219     last_time_moving_ = ros::Time::now().toSec();
00220 
00221     getRobotPoseGlobal();
00222     x_last_ = robot_pose_global_.pose.position.x;
00223     y_last_ = robot_pose_global_.pose.position.y;
00224     theta_last_ = tf::getYaw(robot_pose_global_.pose.orientation);
00225     vtheta_last_ = 0.0f;
00226     vx_last_ = 0.0f;
00227     vy_last_ = 0.0f;
00228     last_time_ = ros::Time::now().toSec();
00229 
00230     goal_pose_global_ = transformGoalToMap(goal->target_pose);
00231 
00232     move_ = true;
00233 
00234     ros::Rate loop_rate(100);
00235     while(nh_.ok()) {
00236       loop_rate.sleep();
00237 
00238       if(as_.isPreemptRequested()) {
00239         if(as_.isNewGoalAvailable()) {
00240           ROS_INFO("New goal received, updating movement");
00241           //if we're active and a new goal is available, we'll accept it
00242           move_base_msgs::MoveBaseGoal new_goal = * as_.acceptNewGoal();
00243           goal_pose_global_ = transformGoalToMap(new_goal.target_pose);
00244 
00245           last_time_moving_ = ros::Time::now().toSec();
00246           move_ = true;
00247         } else {
00248           ROS_INFO("Preempt requested, aborting movement");
00249           //notify the ActionServer that we've successfully preempted
00250           as_.setPreempted();
00251 
00252           move_ = false;
00253           stopMovement();
00254           return;
00255         }
00256       }
00257 
00258       performControllerStep();
00259 
00260       if(finished_) {
00261         as_.setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
00262         ROS_INFO("Goal reached.");
00263         return;
00264       }
00265 
00266       if(!as_.isActive()) {
00267         ROS_INFO("Goal not active anymore. Stop!");
00268         return;
00269       }
00270     }
00271 
00272 
00273     //if the node is killed then we'll abort and return
00274     stopMovement();
00275     as_.setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
00276     return;
00277   };
00278 
00279 
00280   void odometryCB(const nav_msgs::Odometry::ConstPtr &odometry){
00281     geometry_msgs::Vector3Stamped vec_stamped;
00282 
00283     vec_stamped.vector = odometry->twist.twist.linear;
00284     vec_stamped.header.frame_id =  "base_footprint";
00285     try
00286     {
00287       tf_listener_.waitForTransform(robot_frame_, vec_stamped.header.frame_id, ros::Time(0), ros::Duration(1.0));
00288       tf_listener_.transformVector(robot_frame_, vec_stamped, robot_twist_linear_robot_);
00289     }
00290     catch(tf::TransformException& ex){ROS_ERROR("%s",ex.what());}
00291 
00292     vec_stamped.vector = odometry->twist.twist.angular;
00293     vec_stamped.header.frame_id =  "base_footprint";
00294     try
00295     {
00296       tf_listener_.waitForTransform(robot_frame_, vec_stamped.header.frame_id, ros::Time(0), ros::Duration(1.0));
00297       tf_listener_.transformVector(robot_frame_, vec_stamped, robot_twist_angular_robot_);
00298     }
00299     catch(tf::TransformException& ex){ROS_ERROR("%s",ex.what());}
00300   }
00301 
00302   bool serviceCB(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
00303   {
00304     global_frame_ = req.data;
00305     ros::NodeHandle private_nh("~");
00306     private_nh.setParam("global_frame", req.data);
00307     ROS_INFO_STREAM("Set global frame to: " << req.data);
00308     res.success = true;
00309     res.message = "";
00310 
00311     return true;
00312   }
00313 
00314   // Destructor
00315   ~NodeClass()
00316   {
00317   }
00318 
00319   void performControllerStep();
00320   bool getUseMoveAction(void);
00321 
00322 private:
00323   tf::TransformListener tf_listener_;
00324   std::string global_frame_, robot_frame_;
00325   geometry_msgs::PoseStamped goal_pose_global_;
00326   geometry_msgs::PoseStamped zero_pose_;
00327   geometry_msgs::PoseStamped robot_pose_global_;
00328   geometry_msgs::Vector3Stamped robot_twist_linear_robot_, robot_twist_angular_robot_;
00329 
00330   double slow_down_distance_, goal_abortion_time_;
00331 
00332   bool finished_, move_;
00333 
00334   bool use_move_action_;
00335 
00336   pthread_mutex_t m_mutex;
00337 
00338   //core functions:
00339   void publishVelocitiesGlobal(double vx, double vy, double theta);
00340   geometry_msgs::PoseStamped transformGoalToMap(geometry_msgs::PoseStamped goal_pose);
00341   geometry_msgs::PoseStamped getRobotPoseGlobal();
00342 
00343   //helper functions:
00344   double getDistance2d(geometry_msgs::PoseStamped a, geometry_msgs::PoseStamped b);
00345   double getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b);
00346   double getThetaDiffRad(double target, double actual);
00347   double sign(double x);
00348   void stopMovement();
00349   bool notMovingDueToObstacle();
00350   bool goalValid(const geometry_msgs::PoseStamped& goal_pose);
00351 
00352   //Potential field Controller variables
00353   double vx_last_, vy_last_, x_last_, y_last_, theta_last_, vtheta_last_;
00354   double goal_threshold_, speed_threshold_;
00355   double goal_threshold_rot_, speed_threshold_rot_;
00356   double kp_, kv_, virt_mass_;
00357   double kp_rot_, kv_rot_, virt_mass_rot_;
00358   double last_time_;
00359   double v_max_, vtheta_max_;
00360   double last_time_moving_;
00361 
00362 }; //NodeClass
00363 
00364 bool NodeClass::getUseMoveAction(void)
00365 {
00366   return use_move_action_;
00367 }
00368 
00369 geometry_msgs::PoseStamped NodeClass::transformGoalToMap(geometry_msgs::PoseStamped goal_pose) {
00370   geometry_msgs::PoseStamped goal_global_;
00371   if(goal_pose.header.frame_id == global_frame_) return goal_pose;
00372   else if(tf_listener_.canTransform(global_frame_, goal_pose.header.frame_id, ros::Time(0), new std::string)) {
00373     tf_listener_.transformPose(global_frame_, ros::Time(0), goal_pose, "base_link", goal_global_);
00374     return goal_global_;
00375   } else {
00376     ROS_WARN("Can't transform goal to global frame %s", global_frame_.c_str());
00377     return robot_pose_global_;
00378   }
00379 }
00380 
00381 geometry_msgs::PoseStamped NodeClass::getRobotPoseGlobal() {
00382   try
00383   {
00384     ros::Time now = ros::Time::now();
00385     tf_listener_.waitForTransform(global_frame_, robot_frame_, now, ros::Duration(5.0));
00386     tf_listener_.transformPose(global_frame_, now, zero_pose_, robot_frame_, robot_pose_global_);
00387   }
00388   catch(tf::TransformException& ex){
00389     ROS_WARN("Failed to find robot pose in global frame %s", global_frame_.c_str());
00390     robot_pose_global_ = zero_pose_;
00391     return zero_pose_;
00392   }
00393 
00394   return robot_pose_global_;
00395 }
00396 
00397 double NodeClass::getDistance2d(geometry_msgs::PoseStamped a, geometry_msgs::PoseStamped b) {
00398   return sqrt( pow(a.pose.position.x - b.pose.position.x,2) + pow(a.pose.position.y - b.pose.position.y,2) );
00399 }
00400 
00401 double NodeClass::getDistance2d(geometry_msgs::Point a, geometry_msgs::Point b) {
00402   return sqrt( pow(a.x - b.x,2) + pow(a.y - b.y,2) );
00403 }
00404 
00405 double NodeClass::sign(double x) {
00406   if(x >= 0.0f) return 1.0f;
00407   else return -1.0f;
00408 }
00409 
00410 double NodeClass::getThetaDiffRad(double target, double actual) {
00411   return angles::shortest_angular_distance(actual, target);
00412 }
00413 
00414 void NodeClass::publishVelocitiesGlobal(double vx, double vy, double theta) {
00415   //Transform commands from global frame to robot coordinate system
00416   geometry_msgs::Vector3Stamped cmd_global, cmd_robot;
00417   geometry_msgs::Twist msg;
00418 
00419   cmd_global.header.frame_id = global_frame_;
00420   cmd_global.header.stamp = ros::Time::now();
00421   cmd_global.vector.x = vx;
00422   cmd_global.vector.y = vy;
00423   try
00424   {
00425     tf_listener_.waitForTransform(robot_frame_, cmd_global.header.frame_id, cmd_global.header.stamp, ros::Duration(1.0));
00426     tf_listener_.transformVector(robot_frame_, cmd_global, cmd_robot);
00427   } catch (tf::TransformException ex){
00428     ROS_ERROR("%s",ex.what());
00429     cmd_robot.vector.x = 0.0f;
00430     cmd_robot.vector.y = 0.0f;
00431   }
00432 
00433   // make sure that the published velocities are in an at least somewhat reasonable range
00434   try
00435   {
00436     if ( std::isnan(cmd_robot.vector.x) || std::isnan(cmd_robot.vector.y) || std::isnan(theta) )
00437     {
00438       std::string err = "linear_nav: Output velocity contains NaN!";
00439       throw err;
00440     }
00441     if ( ! ( fabs(cmd_robot.vector.x) < 5.0 && fabs(cmd_robot.vector.y) < 5.0 && fabs(theta) < M_PI ) )
00442     {
00443       std::string err = "linear_nav: Output velocity too high";
00444       throw err;
00445     }
00446   }
00447   catch ( std::string err )
00448   {
00449     ROS_ERROR("%s", err.c_str());
00450     topic_pub_command_.publish(geometry_msgs::Twist());
00451     return;
00452   }
00453 
00454 
00455   msg.linear = cmd_robot.vector;
00456   msg.angular.z = theta;
00457   msg.linear.z = 0.0; msg.angular.x = 0.0; msg.angular.y = 0.0;
00458 
00459   topic_pub_command_.publish(msg);
00460 }
00461 
00462 void NodeClass::stopMovement() {
00463   publishVelocitiesGlobal(0.0f, 0.0f, 0.0f);
00464   vx_last_ = 0.0f;
00465   vy_last_ = 0.0f;
00466   vtheta_last_ = 0.0f;
00467 }
00468 
00469 bool NodeClass::notMovingDueToObstacle() {
00470   if (move_ == true && // should move
00471       finished_ == false && // has not reached goal
00472       fabs(robot_twist_linear_robot_.vector.x) <= 0.01 && // velocity components are small
00473       fabs(robot_twist_linear_robot_.vector.y) <= 0.01 &&
00474       fabs(robot_twist_angular_robot_.vector.z) <= 0.01 &&
00475       ros::Time::now().toSec() - last_time_moving_ > goal_abortion_time_ ) // has not been moving for x seconds
00476   {
00477     return true;
00478   } else if ( fabs(robot_twist_linear_robot_.vector.x) > 0.01 ||
00479               fabs(robot_twist_linear_robot_.vector.y) > 0.01 ||
00480               fabs(robot_twist_angular_robot_.vector.z) > 0.01 )
00481   { // still moving, then update last_time_moving_
00482     last_time_moving_ = ros::Time::now().toSec();
00483   }
00484 
00485   return false;
00486 }
00487 
00488 bool NodeClass::goalValid(const geometry_msgs::PoseStamped& goal_pose)
00489 {
00490   if( goal_pose.pose.orientation.x == 0.0 &&
00491       goal_pose.pose.orientation.y == 0.0 &&
00492       goal_pose.pose.orientation.z == 0.0 &&
00493       goal_pose.pose.orientation.w == 0.0 )
00494   {
00495     ROS_WARN("Goal invalid! Received Quaternion with all values 0.0!");
00496     return false;
00497   }
00498   else if (!tf_listener_.waitForTransform(global_frame_, goal_pose.header.frame_id, goal_pose.header.stamp, ros::Duration(1.0)))
00499   {
00500     ROS_WARN_STREAM("Can not transform goal which is given in /"
00501                     << goal_pose.header.frame_id << " into global frame /" << global_frame_);
00502     return false;
00503   }
00504   else if (!tf_listener_.waitForTransform(robot_frame_, goal_pose.header.frame_id, goal_pose.header.stamp, ros::Duration(1.0)))
00505   {
00506      ROS_WARN_STREAM("Can not transform goal which is given in /"
00507                       << goal_pose.header.frame_id << " into robot frame /" << robot_frame_);
00508      return false;
00509   }
00510   else
00511   {
00512     return true;
00513   }
00514 }
00515 
00516 void NodeClass::performControllerStep() {
00517   pthread_mutex_lock(&m_mutex);
00518 
00519   double dt;
00520   double F_x, F_y, F_theta;
00521   double distance_to_goal;
00522   double theta, theta_goal;
00523   double cmd_vx, cmd_vy, cmd_vtheta;
00524   double vx_d, vy_d, vtheta_d, v_factor;
00525   double v_max_goal = v_max_;
00526 
00527   if(!move_) {
00528     if(!use_move_action_)
00529       last_time_ = ros::Time::now().toSec();
00530     pthread_mutex_unlock(&m_mutex);
00531     return;
00532   }
00533 
00534   getRobotPoseGlobal();
00535 
00536   distance_to_goal = getDistance2d(robot_pose_global_, goal_pose_global_);
00537   theta = tf::getYaw(robot_pose_global_.pose.orientation);
00538   theta_goal = tf::getYaw(goal_pose_global_.pose.orientation);
00539 
00540   //exit, if positions and velocities lie inside thresholds
00541   if( distance_to_goal <= goal_threshold_ &&
00542       sqrt(vx_last_ * vx_last_ + vy_last_ * vy_last_) <= speed_threshold_ &&
00543       fabs(getThetaDiffRad(theta_goal, theta)) <= goal_threshold_rot_ &&
00544       fabs(vtheta_last_) <= speed_threshold_rot_ )
00545   {
00546     finished_ = true;
00547     move_ = false;
00548     stopMovement();
00549     if(!use_move_action_)
00550       last_time_ = ros::Time::now().toSec();
00551     pthread_mutex_unlock(&m_mutex);
00552     return;
00553   } else if( notMovingDueToObstacle() == true ) {
00554     finished_ = false;
00555     move_ = false;
00556     stopMovement();
00557     if(use_move_action_)
00558     {
00559       as_.setAborted(move_base_msgs::MoveBaseResult(), "Cancel the goal because an obstacle is blocking the path.");
00560     }
00561     else
00562     {
00563       last_time_ = ros::Time::now().toSec();
00564     }
00565     ROS_INFO("Cancel the goal because an obstacle is blocking the path.");
00566     pthread_mutex_unlock(&m_mutex);
00567     return;
00568   } else finished_ = false;
00569 
00570   dt = ros::Time::now().toSec() - last_time_;
00571   last_time_ = ros::Time::now().toSec();
00572 
00573   //Slow down while approaching goal
00574   if(distance_to_goal < slow_down_distance_) {
00575     //implementation for linear decrease of v_max:
00576     double goal_linear_slope = v_max_ / slow_down_distance_;
00577     v_max_goal = distance_to_goal * goal_linear_slope;
00578 
00579     if(v_max_goal > v_max_) v_max_goal = v_max_;
00580       else if(v_max_goal < 0.0f) v_max_goal = 0.0f;
00581   }
00582 
00583   //Translational movement
00584   //calculation of v factor to limit maxspeed
00585   vx_d = kp_/kv_ * (goal_pose_global_.pose.position.x - robot_pose_global_.pose.position.x);
00586   vy_d = kp_/kv_ * (goal_pose_global_.pose.position.y - robot_pose_global_.pose.position.y);
00587   v_factor = v_max_goal / sqrt(vy_d*vy_d + vx_d*vx_d);
00588 
00589   if(v_factor > 1.0) v_factor = 1.0;
00590 
00591   F_x = - kv_ * vx_last_ + v_factor * kp_ * (goal_pose_global_.pose.position.x - robot_pose_global_.pose.position.x);
00592   F_y = - kv_ * vy_last_ + v_factor * kp_ * (goal_pose_global_.pose.position.y - robot_pose_global_.pose.position.y);
00593 
00594   cmd_vx = vx_last_ + F_x / virt_mass_ * dt;
00595   cmd_vy = vy_last_ + F_y / virt_mass_ * dt;
00596 
00597   //Rotational Movement
00598   //calculation of v factor to limit maxspeed
00599   vtheta_d = kp_rot_ / kv_rot_ * getThetaDiffRad(theta_goal, theta);
00600   v_factor = fabs(vtheta_max_ / vtheta_d);
00601   if(v_factor > 1.0) v_factor = 1.0;
00602 
00603   F_theta = - kv_rot_ * vtheta_last_ + v_factor * kp_rot_ * getThetaDiffRad(theta_goal, theta);
00604   cmd_vtheta = vtheta_last_ + F_theta / virt_mass_rot_ * dt;
00605 
00606   //Publish velocities, these calculated forces and velocities are for the global frame
00607   //they are transformed to robot_frame later
00608   x_last_ = robot_pose_global_.pose.position.x;
00609   y_last_ = robot_pose_global_.pose.position.y;
00610   theta_last_ = theta;
00611   vx_last_ = cmd_vx;
00612   vy_last_ = cmd_vy;
00613   vtheta_last_ = cmd_vtheta;
00614 
00615   publishVelocitiesGlobal(cmd_vx, cmd_vy, cmd_vtheta);
00616   pthread_mutex_unlock(&m_mutex);
00617 }
00618 
00619 
00620 
00621 //#######################
00622 //#### main programm ####
00623 int main(int argc, char** argv)
00624 {
00625   // initialize ROS, spezify name of node
00626   ros::init(argc, argv, "cob_linear");
00627 
00628   // create nodeClass
00629   NodeClass nodeClass("move_base_linear");
00630 
00631   if(nodeClass.getUseMoveAction())
00632   {
00633     ros::spin();
00634   }
00635   else
00636   {
00637     ros::Rate loop_rate(100);
00638     while(nodeClass.nh_.ok())
00639     {
00640       nodeClass.performControllerStep();
00641       loop_rate.sleep();
00642       ros::spinOnce();
00643     }
00644   }
00645 
00646   return 0;
00647 }
00648 


cob_linear_nav
Author(s): Matthias Gruhler , Philipp Koehler
autogenerated on Thu Jun 6 2019 21:01:16