teb_local_planner_ros.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann
00037  *********************************************************************/
00038 
00039 #ifndef TEB_LOCAL_PLANNER_ROS_H_
00040 #define TEB_LOCAL_PLANNER_ROS_H_
00041 
00042 #include <ros/ros.h>
00043 
00044 // base local planner base class and utilities
00045 #include <nav_core/base_local_planner.h>
00046 #include <base_local_planner/goal_functions.h>
00047 #include <base_local_planner/odometry_helper_ros.h>
00048 #include <base_local_planner/costmap_model.h>
00049 
00050 
00051 // timed-elastic-band related classes
00052 #include <teb_local_planner/optimal_planner.h>
00053 #include <teb_local_planner/homotopy_class_planner.h>
00054 #include <teb_local_planner/visualization.h>
00055 
00056 // message types
00057 #include <nav_msgs/Path.h>
00058 #include <nav_msgs/Odometry.h>
00059 #include <geometry_msgs/PoseStamped.h>
00060 #include <visualization_msgs/MarkerArray.h>
00061 #include <visualization_msgs/Marker.h>
00062 #include <teb_local_planner/ObstacleMsg.h>
00063 
00064 // transforms
00065 #include <tf/tf.h>
00066 #include <tf/transform_listener.h>
00067 #include <tf/transform_datatypes.h>
00068 
00069 // costmap
00070 #include <costmap_2d/costmap_2d_ros.h>
00071 #include <costmap_converter/costmap_converter_interface.h>
00072 
00073 
00074 // dynamic reconfigure
00075 #include <teb_local_planner/TebLocalPlannerReconfigureConfig.h>
00076 #include <dynamic_reconfigure/server.h>
00077 
00078 // boost classes
00079 #include <boost/bind.hpp>
00080 #include <boost/shared_ptr.hpp>
00081 
00082 
00083 namespace teb_local_planner
00084 {
00085 
00091 class TebLocalPlannerROS : public nav_core::BaseLocalPlanner
00092 {
00093 
00094 public:
00098   TebLocalPlannerROS();
00099 
00103   ~TebLocalPlannerROS();
00104 
00111   void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);
00112 
00118   bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
00119 
00125   bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
00126 
00134   bool isGoalReached();
00135   
00136   
00137     
00140   
00148   static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);
00149 
00155   static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh);
00156   
00167   static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);
00168   
00178   static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
00179   
00181   
00182 protected:
00183 
00192   void updateObstacleContainerWithCostmap();
00193   
00200   void updateObstacleContainerWithCostmapConverter();
00201   
00207   void updateObstacleContainerWithCustomObstacles();
00208 
00209 
00216   void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation);
00217   
00218   
00226   void reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level);
00227   
00228   
00233   void customObstacleCB(const teb_local_planner::ObstacleMsg::ConstPtr& obst_msg);
00234   
00235   
00251   bool pruneGlobalPlan(const tf::TransformListener& tf, const tf::Stamped<tf::Pose>& global_pose, 
00252                        std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot=1);
00253   
00271   bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
00272                            const tf::Stamped<tf::Pose>& global_pose,  const costmap_2d::Costmap2D& costmap,
00273                            const std::string& global_frame, double max_plan_length, std::vector<geometry_msgs::PoseStamped>& transformed_plan,
00274                            int* current_goal_idx = NULL, tf::StampedTransform* tf_plan_to_global = NULL) const;
00275     
00291   double estimateLocalGoalOrientation(const std::vector<geometry_msgs::PoseStamped>& global_plan, const tf::Stamped<tf::Pose>& local_goal,
00292                                       int current_goal_idx, const tf::StampedTransform& tf_plan_to_global, int moving_average_length=3) const;
00293         
00294         
00309   void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
00310                         double max_vel_theta, double max_vel_x_backwards) const;
00311 
00312   
00327   double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const;
00328   
00338   void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
00339   
00340   
00341   void configureBackupModes(std::vector<geometry_msgs::PoseStamped>& transformed_plan,  int& goal_idx);
00342 
00343 
00344   
00345 private:
00346   // Definition of member variables
00347 
00348   // external objects (store weak pointers)
00349   costmap_2d::Costmap2DROS* costmap_ros_; 
00350   costmap_2d::Costmap2D* costmap_; 
00351   tf::TransformListener* tf_; 
00352     
00353   // internal objects (memory management owned)
00354   PlannerInterfacePtr planner_; 
00355   ObstContainer obstacles_; 
00356   ViaPointContainer via_points_; 
00357   TebVisualizationPtr visualization_; 
00358   boost::shared_ptr<base_local_planner::CostmapModel> costmap_model_;  
00359   TebConfig cfg_; 
00360   
00361   std::vector<geometry_msgs::PoseStamped> global_plan_; 
00362   
00363   base_local_planner::OdometryHelperRos odom_helper_; 
00364   
00365   pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> costmap_converter_loader_; 
00366   boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> costmap_converter_; 
00367 
00368   boost::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg_; 
00369   ros::Subscriber custom_obst_sub_; 
00370   boost::mutex custom_obst_mutex_; 
00371   ObstacleMsg custom_obstacle_msg_; 
00372   
00373   PoseSE2 robot_pose_; 
00374   PoseSE2 robot_goal_; 
00375   geometry_msgs::Twist robot_vel_; 
00376   bool goal_reached_; 
00377   ros::Time time_last_infeasible_plan_; 
00378   int no_infeasible_plans_; 
00379   
00380   std::vector<geometry_msgs::Point> footprint_spec_; 
00381   double robot_inscribed_radius_; 
00382   double robot_circumscribed_radius; 
00383   
00384   std::string global_frame_; 
00385   std::string robot_base_frame_; 
00386     
00387   // flags
00388   bool initialized_; 
00389 
00390 public:
00391   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00392 };
00393   
00394 }; // end namespace teb_local_planner
00395 
00396 #endif // TEB_LOCAL_PLANNER_ROS_H_
00397 
00398 


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Sat Jun 8 2019 20:21:34