teb_local_planner_ros.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the institute nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christoph Rösmann
 *********************************************************************/

#ifndef TEB_LOCAL_PLANNER_ROS_H_
#define TEB_LOCAL_PLANNER_ROS_H_

#include <ros/ros.h>

// base local planner base class and utilities
#include <nav_core/base_local_planner.h>
#include <base_local_planner/goal_functions.h>
#include <base_local_planner/odometry_helper_ros.h>
#include <base_local_planner/costmap_model.h>


// timed-elastic-band related classes
#include <teb_local_planner/optimal_planner.h>
#include <teb_local_planner/homotopy_class_planner.h>
#include <teb_local_planner/visualization.h>
#include <teb_local_planner/recovery_behaviors.h>

// message types
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <costmap_converter/ObstacleMsg.h>

// transforms
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

// costmap
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_converter/costmap_converter_interface.h>


// dynamic reconfigure
#include <teb_local_planner/TebLocalPlannerReconfigureConfig.h>
#include <dynamic_reconfigure/server.h>

// boost classes
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>


namespace teb_local_planner
{

class TebLocalPlannerROS : public nav_core::BaseLocalPlanner
{

public:
  TebLocalPlannerROS();

  ~TebLocalPlannerROS();

  void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);

  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);

  bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);

  bool isGoalReached();
  
  
    
  
  static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);

  static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh);
  
  static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);
  
  static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
  
  
protected:

  void updateObstacleContainerWithCostmap();
  
  void updateObstacleContainerWithCostmapConverter();
  
  void updateObstacleContainerWithCustomObstacles();


  void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation);
  
  
  void reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level);
  
  
  void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
  
  void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg);

  bool pruneGlobalPlan(const tf::TransformListener& tf, const tf::Stamped<tf::Pose>& global_pose, 
                       std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot=1);
  
  bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
                           const tf::Stamped<tf::Pose>& global_pose,  const costmap_2d::Costmap2D& costmap,
                           const std::string& global_frame, double max_plan_length, std::vector<geometry_msgs::PoseStamped>& transformed_plan,
                           int* current_goal_idx = NULL, tf::StampedTransform* tf_plan_to_global = NULL) const;
    
  double estimateLocalGoalOrientation(const std::vector<geometry_msgs::PoseStamped>& global_plan, const tf::Stamped<tf::Pose>& local_goal,
                                      int current_goal_idx, const tf::StampedTransform& tf_plan_to_global, int moving_average_length=3) const;
        
        
  void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
                        double max_vel_theta, double max_vel_x_backwards) const;

  
  double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const;
  
  void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
  
  
  void configureBackupModes(std::vector<geometry_msgs::PoseStamped>& transformed_plan,  int& goal_idx);


  
private:
  // Definition of member variables

  // external objects (store weak pointers)
  costmap_2d::Costmap2DROS* costmap_ros_; 
  costmap_2d::Costmap2D* costmap_; 
  tf::TransformListener* tf_; 
    
  // internal objects (memory management owned)
  PlannerInterfacePtr planner_; 
  ObstContainer obstacles_; 
  ViaPointContainer via_points_; 
  TebVisualizationPtr visualization_; 
  boost::shared_ptr<base_local_planner::CostmapModel> costmap_model_;  
  TebConfig cfg_; 
  FailureDetector failure_detector_; 
  
  std::vector<geometry_msgs::PoseStamped> global_plan_; 
  
  base_local_planner::OdometryHelperRos odom_helper_; 
  
  pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> costmap_converter_loader_; 
  boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> costmap_converter_; 

  boost::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > dynamic_recfg_; 
  ros::Subscriber custom_obst_sub_; 
  boost::mutex custom_obst_mutex_; 
  costmap_converter::ObstacleArrayMsg custom_obstacle_msg_; 

  ros::Subscriber via_points_sub_; 
  bool custom_via_points_active_; 
  boost::mutex via_point_mutex_; 

  PoseSE2 robot_pose_; 
  PoseSE2 robot_goal_; 
  geometry_msgs::Twist robot_vel_; 
  bool goal_reached_; 
  ros::Time time_last_infeasible_plan_; 
  int no_infeasible_plans_; 
  ros::Time time_last_oscillation_; 
  RotType last_preferred_rotdir_; 
  geometry_msgs::Twist last_cmd_; 
  
  std::vector<geometry_msgs::Point> footprint_spec_; 
  double robot_inscribed_radius_; 
  double robot_circumscribed_radius; 
  
  std::string global_frame_; 
  std::string robot_base_frame_; 
    
  // flags
  bool initialized_; 

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
  
}; // end namespace teb_local_planner

#endif // TEB_LOCAL_PLANNER_ROS_H_