hector_path_follower: hector_path_follower: hector_path

00001 //=================================================================================================
00002 // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
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 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 /*********************************************************************
00030 * Based heavily on the pose_follower package
00031 *********************************************************************/
00032 #ifndef HECTOR_PATH_FOLLOWER_H_
00033 #define HECTOR_PATH_FOLLOWER_H_
00034 #include <ros/ros.h>
00035 #include <tf/tf.h>
00036 #include <tf/transform_datatypes.h>
00037 #include <tf/transform_listener.h>
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <geometry_msgs/Twist.h>
00040 
00041 namespace pose_follower {
00042   class HectorPathFollower
00043   {
00044     public:
00045       HectorPathFollower();
00046       void initialize(tf::TransformListener* tf);
00047       bool isGoalReached();
00048       bool setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan);
00049       bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
00050 
00051     private:
00052       inline double sign(double n){
00053         return n < 0.0 ? -1.0 : 1.0;
00054       }
00055 
00056       geometry_msgs::Twist diff2D(const tf::Pose& pose1, const tf::Pose&  pose2);
00057       geometry_msgs::Twist limitTwist(const geometry_msgs::Twist& twist);
00058       double headingDiff(double pt_x, double pt_y, double x, double y, double heading);
00059 
00060       bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan, const std::string& global_frame,
00061           std::vector<geometry_msgs::PoseStamped>& transformed_plan);
00062 
00063       //void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
00064       bool stopped();
00065 
00066       bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const;
00067 
00068       tf::TransformListener* tf_;
00069 
00070       double K_trans_, K_rot_, tolerance_trans_, tolerance_rot_;
00071       double tolerance_timeout_;
00072       double max_vel_lin_, max_vel_th_;
00073       double min_vel_lin_, min_vel_th_;
00074       double min_in_place_vel_th_, in_place_trans_vel_;
00075       bool holonomic_;
00076       std::string p_robot_base_frame_;
00077       std::string p_global_frame_;
00078 
00079       boost::mutex odom_lock_;
00080       ros::Subscriber odom_sub_;
00081       //nav_msgs::Odometry base_odom_;
00082       double trans_stopped_velocity_, rot_stopped_velocity_;
00083       ros::Time goal_reached_time_;
00084       unsigned int current_waypoint_; 
00085       std::vector<geometry_msgs::PoseStamped> global_plan_;
00086       //base_local_planner::TrajectoryPlannerROS collision_planner_;
00087       int samples_;
00088   };
00089 };
00090 #endif
hector_path_follower.h
