pose_base_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2009, 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: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include <pose_base_controller/pose_base_controller.h>
00038 
00039 namespace pose_base_controller {
00040   PoseBaseController::PoseBaseController() : action_server_(ros::NodeHandle(), 
00041                                         "pose_base_controller", 
00042                                         boost::bind(&PoseBaseController::execute, this, _1),
00043                                         false){
00044     ros::NodeHandle node_private("~");
00045     node_private.param("k_trans", K_trans_, 1.0);
00046     node_private.param("k_rot", K_rot_, 1.0);
00047 
00048     node_private.param("tolerance_trans", tolerance_trans_, 0.02);
00049     node_private.param("tolerance_rot", tolerance_rot_, 0.04);
00050     node_private.param("tolerance_timeout", tolerance_timeout_, 0.5);
00051 
00052     node_private.param("fixed_frame", fixed_frame_, std::string("odom_combined"));
00053     node_private.param("base_frame", base_frame_, std::string("base_link"));
00054 
00055     node_private.param("holonomic", holonomic_, true);
00056 
00057     node_private.param("max_vel_lin", max_vel_lin_, 0.9);
00058     node_private.param("max_vel_th", max_vel_th_, 1.4);
00059 
00060     node_private.param("min_vel_lin", min_vel_lin_, 0.0);
00061     node_private.param("min_vel_th", min_vel_th_, 0.0);
00062     node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
00063     node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
00064     node_private.param("frequency", freq_, 100.0);
00065     node_private.param("transform_tolerance", transform_tolerance_, 0.5);
00066 
00067     node_private.param("trans_stopped_velocity", trans_stopped_velocity_, 1e-4);
00068     node_private.param("rot_stopped_velocity", rot_stopped_velocity_, 1e-4);
00069 
00070     ros::NodeHandle node;
00071     odom_sub_ = node.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&PoseBaseController::odomCallback, this, _1));
00072     vel_pub_ = node.advertise<geometry_msgs::Twist>("base_controller/command", 10);
00073 
00074     action_server_.start();
00075     ROS_DEBUG("Started server");
00076   }
00077 
00078   void PoseBaseController::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
00079     //we assume that the odometry is published in the frame of the base
00080     boost::mutex::scoped_lock lock(odom_lock_);
00081     base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
00082     base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
00083     base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
00084     ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
00085         base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
00086   }
00087 
00088   double PoseBaseController::headingDiff(double x, double y, double pt_x, double pt_y, double heading)
00089   {
00090     double v1_x = x - pt_x;
00091     double v1_y = y - pt_y;
00092     double v2_x = cos(heading);
00093     double v2_y = sin(heading);
00094 
00095     double perp_dot = v1_x * v2_y - v1_y * v2_x;
00096     double dot = v1_x * v2_x + v1_y * v2_y;
00097 
00098     //get the signed angle
00099     double vector_angle = atan2(perp_dot, dot);
00100 
00101     return -1.0 * vector_angle;
00102   }
00103 
00104   tf::Stamped<tf::Pose> PoseBaseController::getRobotPose(){
00105     tf::Stamped<tf::Pose> global_pose, robot_pose;
00106     global_pose.setIdentity();
00107     robot_pose.setIdentity();
00108     robot_pose.frame_id_ = base_frame_;
00109     robot_pose.stamp_ = ros::Time();
00110 
00111     tf_.transformPose(fixed_frame_, robot_pose, global_pose);
00112     //ROS_INFO("Delay: %f", (global_pose.stamp_ - ros::Time::now()).toSec());
00113     return global_pose;
00114   }
00115 
00116   move_base_msgs::MoveBaseGoal PoseBaseController::goalToFixedFrame(const move_base_msgs::MoveBaseGoal& goal){
00117     tf::Stamped<tf::Pose> pose, fixed_pose;
00118     tf::poseStampedMsgToTF(goal.target_pose, pose);
00119 
00120     //just get the latest available transform... for accuracy they should send
00121     //goals in the frame of the planner
00122     pose.stamp_ = ros::Time();
00123 
00124     try{
00125       tf_.transformPose(fixed_frame_, pose, fixed_pose);
00126     }
00127     catch(tf::TransformException& ex){
00128       ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
00129           pose.frame_id_.c_str(), fixed_frame_.c_str(), ex.what());
00130       return goal;
00131     }
00132 
00133     move_base_msgs::MoveBaseGoal fixed_goal;
00134     tf::poseStampedTFToMsg(fixed_pose, fixed_goal.target_pose);
00135     return fixed_goal;
00136 
00137   }
00138 
00139   void PoseBaseController::execute(const move_base_msgs::MoveBaseGoalConstPtr& user_goal){
00140     bool success = false;
00141 
00142     move_base_msgs::MoveBaseGoal goal = goalToFixedFrame(*user_goal);
00143 
00144     success = controlLoop(goal);
00145 
00146     //based on the control loop's exit status... we'll set our goal status
00147     if(success){
00148       //wait until we're stopped before returning success
00149       ros::Rate r(10.0);
00150       while(!stopped()){
00151         geometry_msgs::Twist empty_twist;
00152         vel_pub_.publish(empty_twist);
00153         r.sleep();
00154       }
00155       action_server_.setSucceeded();
00156     }
00157     else{
00158       //if a preempt was requested... the control loop exits for that reason
00159       if(action_server_.isPreemptRequested())
00160         action_server_.setPreempted();
00161       else
00162         action_server_.setAborted();
00163     }
00164   }
00165 
00166   bool PoseBaseController::stopped(){
00167     //copy over the odometry information
00168     nav_msgs::Odometry base_odom;
00169     {
00170       boost::mutex::scoped_lock lock(odom_lock_);
00171       base_odom = base_odom_;
00172     }
00173 
00174     return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity_
00175       && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity_
00176       && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity_;
00177   }
00178 
00179   bool PoseBaseController::controlLoop(const move_base_msgs::MoveBaseGoal& current_goal){
00180     if(freq_ == 0.0)
00181       return false;
00182 
00183     ros::Rate r(freq_);
00184     ros::Time goal_reached_time = ros::Time::now();
00185     while(ros::ok()){
00186       if(action_server_.isPreemptRequested()){
00187         return false;
00188       }
00189       ROS_DEBUG("At least looping");
00190       tf::Stamped<tf::Pose> target_pose;
00191       tf::poseStampedMsgToTF(current_goal.target_pose, target_pose);
00192 
00193 
00194       //get the current pose of the robot in the fixed frame
00195       tf::Stamped<tf::Pose> robot_pose;
00196       try {
00197         robot_pose = getRobotPose();
00198         //make sure that the transform to the dance frame isn't too out of date
00199         if(robot_pose.stamp_ + ros::Duration(transform_tolerance_) < ros::Time::now()){
00200           ROS_WARN("The %s frame to %s frame transform is more than %.2f seconds old, will not move",
00201               fixed_frame_.c_str(), base_frame_.c_str(), transform_tolerance_);
00202           geometry_msgs::Twist empty_twist;
00203           vel_pub_.publish(empty_twist);
00204           return false;
00205         }
00206       }
00207       catch(tf::TransformException &ex){
00208         ROS_ERROR("Can't transform: %s\n", ex.what());
00209         geometry_msgs::Twist empty_twist;
00210         vel_pub_.publish(empty_twist);
00211         return false;
00212       }
00213       ROS_DEBUG("PoseBaseController: current robot pose %f %f ==> %f", robot_pose.getOrigin().x(), robot_pose.getOrigin().y(), tf::getYaw(robot_pose.getRotation()));
00214       ROS_DEBUG("PoseBaseController: target robot pose %f %f ==> %f", target_pose.getOrigin().x(), target_pose.getOrigin().y(), tf::getYaw(target_pose.getRotation()));
00215 
00216       //get the difference between the two poses
00217       geometry_msgs::Twist diff = diff2D(target_pose, robot_pose);
00218       ROS_DEBUG("PoseBaseController: diff %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
00219 
00220       //publish the desired velocity command to the base
00221       vel_pub_.publish(limitTwist(diff));
00222 
00223       //if we haven't reached our goal... we'll update time
00224       if (fabs(diff.linear.x) > tolerance_trans_ || fabs(diff.linear.y) > tolerance_trans_ || fabs(diff.angular.z) > tolerance_rot_)
00225         goal_reached_time = ros::Time::now();
00226 
00227       //check if we've reached our goal for long enough to succeed
00228       if(goal_reached_time + ros::Duration(tolerance_timeout_) < ros::Time::now()){
00229         geometry_msgs::Twist empty_twist;
00230         vel_pub_.publish(empty_twist);
00231         return true;
00232       }
00233 
00234       r.sleep();
00235     }
00236     return false;
00237   }
00238 
00239   geometry_msgs::Twist PoseBaseController::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
00240   {
00241     geometry_msgs::Twist res;
00242     tf::Pose diff = pose2.inverse() * pose1;
00243     res.linear.x = diff.getOrigin().x();
00244     res.linear.y = diff.getOrigin().y();
00245     res.angular.z = tf::getYaw(diff.getRotation());
00246 
00247     if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
00248       return res;
00249 
00250     //in the case that we're not rotating to our goal position and we have a non-holonomic robot
00251     //we'll need to command a rotational velocity that will help us reach our desired heading
00252     
00253     //we want to compute a goal based on the heading difference between our pose and the target
00254     double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
00255         pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));
00256 
00257     //we'll also check if we can move more effectively backwards
00258     double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(), 
00259         pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));
00260 
00261     //check if its faster to just back up
00262     if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
00263       ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
00264       yaw_diff = neg_yaw_diff;
00265     }
00266 
00267     //compute the desired quaterion
00268     tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
00269     tf::Quaternion rot = pose2.getRotation() * rot_diff;
00270     tf::Pose new_pose = pose1;
00271     new_pose.setRotation(rot);
00272 
00273     diff = pose2.inverse() * new_pose;
00274     res.linear.x = diff.getOrigin().x();
00275     res.linear.y = diff.getOrigin().y();
00276     res.angular.z = tf::getYaw(diff.getRotation());
00277     return res;
00278   }
00279 
00280 
00281   geometry_msgs::Twist PoseBaseController::limitTwist(const geometry_msgs::Twist& twist)
00282   {
00283     geometry_msgs::Twist res = twist;
00284     res.linear.x *= K_trans_;
00285     if(!holonomic_)
00286       res.linear.y = 0.0;
00287     else    
00288       res.linear.y *= K_trans_;
00289     res.angular.z *= K_rot_;
00290 
00291     //make sure to bound things by our velocity limits
00292     double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_;
00293     if (lin_overshoot > 1.0) 
00294     {
00295       res.linear.x /= lin_overshoot;
00296       res.linear.y /= lin_overshoot;
00297     }
00298     if (fabs(res.angular.z) > max_vel_th_) res.angular.z = max_vel_th_ * sign(res.angular.z);
00299     if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z);
00300 
00301     if(fabs(res.linear.x) < in_place_trans_vel_ && fabs(res.linear.y) < in_place_trans_vel_){
00302       if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
00303     }
00304 
00305     ROS_DEBUG("Angular command %f", res.angular.z);
00306     return res;
00307   }
00308 };
00309 
00310 int main(int argc, char** argv)
00311 {
00312   ros::init(argc, argv, "pose_base_controller");
00313 
00314   pose_base_controller::PoseBaseController pbc;
00315 
00316   ros::spin();
00317   return 0;
00318 }


pose_base_controller
Author(s): Eitan Marder-Eppstein
autogenerated on Thu Mar 28 2019 03:37:38