teb_local_planner_ros.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
39 #ifndef TEB_LOCAL_PLANNER_ROS_H_
40 #define TEB_LOCAL_PLANNER_ROS_H_
41 
42 #include <ros/ros.h>
43 
44 // base local planner base class and utilities
49 
50 
51 // timed-elastic-band related classes
56 
57 // message types
58 #include <nav_msgs/Path.h>
59 #include <nav_msgs/Odometry.h>
60 #include <geometry_msgs/PoseStamped.h>
61 #include <visualization_msgs/MarkerArray.h>
62 #include <visualization_msgs/Marker.h>
63 #include <costmap_converter/ObstacleMsg.h>
64 
65 // transforms
66 #include <tf/tf.h>
67 #include <tf/transform_listener.h>
68 #include <tf/transform_datatypes.h>
69 
70 // costmap
73 
74 
75 // dynamic reconfigure
76 #include <teb_local_planner/TebLocalPlannerReconfigureConfig.h>
77 #include <dynamic_reconfigure/server.h>
78 
79 // boost classes
80 #include <boost/bind.hpp>
81 #include <boost/shared_ptr.hpp>
82 
83 
84 namespace teb_local_planner
85 {
86 
93 {
94 
95 public:
100 
105 
112  void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);
113 
119  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
120 
127 
135  bool isGoalReached();
136 
137 
138 
141 
149  static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);
150 
157 
168  static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);
169 
179  static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
180 
182 
183 protected:
184 
194 
202 
209 
210 
217  void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation);
218 
219 
227  void reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level);
228 
229 
234  void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
235 
240  void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg);
241 
257  bool pruneGlobalPlan(const tf::TransformListener& tf, const tf::Stamped<tf::Pose>& global_pose,
258  std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot=1);
259 
277  bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
278  const tf::Stamped<tf::Pose>& global_pose, const costmap_2d::Costmap2D& costmap,
279  const std::string& global_frame, double max_plan_length, std::vector<geometry_msgs::PoseStamped>& transformed_plan,
280  int* current_goal_idx = NULL, tf::StampedTransform* tf_plan_to_global = NULL) const;
281 
297  double estimateLocalGoalOrientation(const std::vector<geometry_msgs::PoseStamped>& global_plan, const tf::Stamped<tf::Pose>& local_goal,
298  int current_goal_idx, const tf::StampedTransform& tf_plan_to_global, int moving_average_length=3) const;
299 
300 
315  void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
316  double max_vel_theta, double max_vel_x_backwards) const;
317 
318 
333  double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const;
334 
344  void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
345 
346 
347  void configureBackupModes(std::vector<geometry_msgs::PoseStamped>& transformed_plan, int& goal_idx);
348 
349 
350 
351 private:
352  // Definition of member variables
353 
354  // external objects (store weak pointers)
358 
359  // internal objects (memory management owned)
367 
368  std::vector<geometry_msgs::PoseStamped> global_plan_;
369 
371 
374 
377  boost::mutex custom_obst_mutex_;
378  costmap_converter::ObstacleArrayMsg custom_obstacle_msg_;
379 
382  boost::mutex via_point_mutex_;
383 
393 
394  std::vector<geometry_msgs::Point> footprint_spec_;
397 
398  std::string global_frame_;
399  std::string robot_base_frame_;
400 
401  // flags
403 
404 public:
405  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
406 };
407 
408 }; // end namespace teb_local_planner
409 
410 #endif // TEB_LOCAL_PLANNER_ROS_H_
411 
412 
boost::shared_ptr< dynamic_reconfigure::Server< TebLocalPlannerReconfigureConfig > > dynamic_recfg_
Dynamic reconfigure server to allow config modifications at runtime.
bool isGoalReached()
Check if the goal pose has been achieved.
TebVisualizationPtr visualization_
Instance of the visualization class (local/global plan, obstacles, ...)
TebLocalPlannerROS()
Default constructor of the teb plugin.
boost::mutex via_point_mutex_
Mutex that locks the via_points container (multi-threaded)
costmap_converter::ObstacleArrayMsg custom_obstacle_msg_
Copy of the most recent obstacle message.
TebConfig config
PoseSE2 robot_goal_
Store current robot goal.
void initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros)
Initializes the teb plugin.
bool initialized_
Keeps track about the correct initialization of this class.
double robot_circumscribed_radius
The radius of the circumscribed circle of the robot.
void updateViaPointsContainer(const std::vector< geometry_msgs::PoseStamped > &transformed_plan, double min_separation)
Update internal via-point container based on the current reference plan.
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)
Prune global plan such that already passed poses are cut off.
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
boost::shared_ptr< base_local_planner::CostmapModel > costmap_model_
bool custom_via_points_active_
Keep track whether valid via-points have been received from via_points_sub_.
void customViaPointsCB(const nav_msgs::Path::ConstPtr &via_points_msg)
Callback for custom via-points.
pluginlib::ClassLoader< costmap_converter::BaseCostmapToPolygons > costmap_converter_loader_
Load costmap converter plugins at runtime.
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
Transforms the global plan of the robot from the planner frame to the local frame (modified)...
std::vector< geometry_msgs::Point > footprint_spec_
Store the footprint of the robot.
int no_infeasible_plans_
Store how many times in a row the planner failed to find a feasible plan.
static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle &nh)
Get the current robot footprint/contour model.
FailureDetector failure_detector_
Detect if the robot got stucked.
bool goal_reached_
store whether the goal is reached or not
std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > Point2dContainer
Abbrev. for a container storing 2d points.
ros::Time time_last_infeasible_plan_
Store at which time stamp the last infeasible plan was detected.
ros::Subscriber custom_obst_sub_
Subscriber for custom obstacles received via a ObstacleMsg.
void updateObstacleContainerWithCostmapConverter()
Update internal obstacle vector based on polygons provided by a costmap_converter plugin...
bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
Given the current position, orientation, and velocity of the robot, compute velocity commands to send...
boost::mutex custom_obst_mutex_
Mutex that locks the obstacle array (multi-threaded)
std::vector< geometry_msgs::PoseStamped > global_plan_
Store the current global plan.
void updateObstacleContainerWithCustomObstacles()
Update internal obstacle vector based on custom messages received via subscriber. ...
RotType
Symbols for left/none/right rotations.
Definition: misc.h:53
costmap_2d::Costmap2DROS * costmap_ros_
Pointer to the costmap ros wrapper, received from the navigation stack.
tf::TransformListener * tf_
pointer to Transform Listener
RotType last_preferred_rotdir_
Store recent preferred turning direction.
~TebLocalPlannerROS()
Destructor of the plugin.
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
Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local...
std::vector< ObstaclePtr > ObstContainer
Abbrev. for containers storing multiple obstacles.
Definition: obstacles.h:297
TebConfig cfg_
Config class that stores and manages all related parameters.
static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue &footprint_xmlrpc, const std::string &full_param_name)
Set the footprint from the given XmlRpcValue.
geometry_msgs::Twist last_cmd_
Store the last control command generated in computeVelocityCommands()
static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose &tf_vel)
Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities...
static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue &value, const std::string &full_param_name)
Get a number from the given XmlRpcValue.
This class implements a pose in the domain SE2: The pose consist of the position x and y and an orie...
Definition: pose_se2.h:57
void configureBackupModes(std::vector< geometry_msgs::PoseStamped > &transformed_plan, int &goal_idx)
PoseSE2 robot_pose_
Store current robot pose.
base_local_planner::OdometryHelperRos odom_helper_
Provides an interface to receive the current velocity from the robot.
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
Saturate the translational and angular velocity to given limits.
costmap_2d::Costmap2D * costmap_
Pointer to the 2d costmap (obtained from the costmap ros wrapper)
Implements the actual abstract navigation stack routines of the teb_local_planner plugin...
This class implements methods in order to detect if the robot got stucked or is oscillating.
void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr &obst_msg)
Callback for custom obstacles that are not obtained from the costmap.
std::string robot_base_frame_
Used as the base frame id of the robot.
void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist)
Validate current parameter values of the footprint for optimization, obstacle distance and the costma...
ros::Subscriber via_points_sub_
Subscriber for custom via-points received via a Path msg.
void updateObstacleContainerWithCostmap()
Update internal obstacle vector based on occupied costmap cells.
double robot_inscribed_radius_
The radius of the inscribed circle of the robot (collision possible)
boost::shared_ptr< costmap_converter::BaseCostmapToPolygons > costmap_converter_
Store the current costmap_converter.
PlannerInterfacePtr planner_
Instance of the underlying optimal planner class.
std::string global_frame_
The frame in which the controller will run.
double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius=0) const
Convert translational and rotational velocities to a steering angle of a carlike robot.
ros::Time time_last_oscillation_
Store at which time stamp the last oscillation was detected.
ObstContainer obstacles_
Obstacle vector that should be considered during local trajectory optimization.
bool setPlan(const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
Set the plan that the teb local planner is following.
ViaPointContainer via_points_
Container of via-points that should be considered during local trajectory optimization.
geometry_msgs::Twist robot_vel_
Store current robot translational and angular velocity (vx, vy, omega)
std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > ViaPointContainer
Typedef for a container storing via-points.
void reconfigureCB(TebLocalPlannerReconfigureConfig &config, uint32_t level)
Callback for the dynamic_reconfigure node.


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 3 2020 04:03:08