visualization.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 VISUALIZATION_H_
40 #define VISUALIZATION_H_
41 
42 
43 
44 // teb stuff
48 
49 // ros stuff
50 #include <ros/publisher.h>
52 
53 // boost
54 #include <boost/graph/adjacency_list.hpp>
55 #include <boost/graph/graph_traits.hpp>
56 
57 // std
58 #include <iterator>
59 
60 // messages
61 #include <nav_msgs/Path.h>
62 #include <geometry_msgs/PoseStamped.h>
63 #include <geometry_msgs/PoseArray.h>
64 #include <tf/transform_datatypes.h>
65 #include <nav_msgs/Odometry.h>
66 #include <nav_msgs/Path.h>
67 #include <visualization_msgs/Marker.h>
68 
69 namespace teb_local_planner
70 {
71 
72 class TebOptimalPlanner;
73 
74 
80 {
81 public:
82 
88 
95 
103  void initialize(ros::NodeHandle& nh, const TebConfig& cfg);
104 
105 
108 
113  void publishGlobalPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan) const;
114 
119  void publishLocalPlan(const std::vector<geometry_msgs::PoseStamped>& local_plan) const;
120 
128  void publishLocalPlanAndPoses(const TimedElasticBand& teb) const;
129 
137  void publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns = "RobotFootprintModel");
138 
144  void publishObstacles(const ObstContainer& obstacles) const;
145 
150  void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns = "ViaPoints") const;
151 
160  template <typename GraphType>
161  void publishGraph(const GraphType& graph, const std::string& ns_prefix = "Graph");
162 
188  template <typename BidirIter>
189  void publishPathContainer(BidirIter first, BidirIter last, const std::string& ns = "PathContainer");
190 
197  void publishTebContainer(const std::vector< boost::shared_ptr<TebOptimalPlanner> >& teb_planner, const std::string& ns = "TebContainer");
198 
209  void publishFeedbackMessage(const std::vector< boost::shared_ptr<TebOptimalPlanner> >& teb_planners, unsigned int selected_trajectory_idx, const ObstContainer& obstacles);
210 
220  void publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles);
221 
223 
224 protected:
225 
230  bool printErrorWhenNotInitialized() const;
231 
237 
238  const TebConfig* cfg_;
239 
241 
242 
243 public:
244  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
245 };
246 
249 
252 
253 
254 } // namespace teb_local_planner
255 
256 
257 // Include template method implementations / definitions
259 
260 #endif /* VISUALIZATION_H_ */
void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > &via_points, const std::string &ns="ViaPoints") const
Publish via-points to the ros topic ../../teb_markers.
const TebConfig * cfg_
Config class that stores and manages all related parameters.
void initialize(ros::NodeHandle &nh, const TebConfig &cfg)
Initializes the class and registers topics.
ros::Publisher feedback_pub_
Publisher for the feedback message for analysis and debug purposes.
void publishLocalPlan(const std::vector< geometry_msgs::PoseStamped > &local_plan) const
Publish a given local plan to the ros topic ../../local_plan.
void publishPathContainer(BidirIter first, BidirIter last, const std::string &ns="PathContainer")
Publish multiple 2D paths (each path given as a point sequence) from a container class.
ros::Publisher teb_poses_pub_
Publisher for the trajectory pose sequence.
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
Class that defines a trajectory modeled as an elastic band with augmented tempoarl information...
Abstract class that defines the interface for robot footprint/contour models.
bool printErrorWhenNotInitialized() const
Small helper function that checks if initialize() has been called and prints an error message if not...
boost::shared_ptr< const TebVisualization > TebVisualizationConstPtr
Abbrev. for shared instances of the TebVisualization (read-only)
void publishGlobalPlan(const std::vector< geometry_msgs::PoseStamped > &global_plan) const
Publish a given global plan to the ros topic ../../global_plan.
void publishLocalPlanAndPoses(const TimedElasticBand &teb) const
Publish Timed_Elastic_Band related stuff (local plan, pose sequence).
void publishObstacles(const ObstContainer &obstacles) const
Publish obstacle positions to the ros topic ../../teb_markers.
void publishTebContainer(const std::vector< boost::shared_ptr< TebOptimalPlanner > > &teb_planner, const std::string &ns="TebContainer")
Publish multiple Tebs from a container class (publish as marker message).
void publishGraph(const GraphType &graph, const std::string &ns_prefix="Graph")
Publish a boost::adjacency_list (boost&#39;s graph datatype) via markers.
boost::shared_ptr< TebVisualization > TebVisualizationPtr
Abbrev. for shared instances of the TebVisualization.
std::vector< ObstaclePtr > ObstContainer
Abbrev. for containers storing multiple obstacles.
Definition: obstacles.h:297
void publishRobotFootprintModel(const PoseSE2 &current_pose, const BaseRobotFootprintModel &robot_model, const std::string &ns="RobotFootprintModel")
Publish the visualization of the robot model.
ros::Publisher teb_marker_pub_
Publisher for visualization markers.
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
TebVisualization()
Default constructor.
ros::Publisher global_plan_pub_
Publisher for the global plan.
This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.
ros::Publisher local_plan_pub_
Publisher for the local plan.
ViaPointContainer via_points
bool initialized_
Keeps track about the correct initialization of this class.
void publishFeedbackMessage(const std::vector< boost::shared_ptr< TebOptimalPlanner > > &teb_planners, unsigned int selected_trajectory_idx, const ObstContainer &obstacles)
Publish a feedback message (multiple trajectory version)


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10