visualization.cpp
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 
41 #include <teb_local_planner/FeedbackMsg.h>
42 
43 namespace teb_local_planner
44 {
45 
46 TebVisualization::TebVisualization() : initialized_(false)
47 {
48 }
49 
51 {
52  initialize(nh, cfg);
53 }
54 
56 {
57  if (initialized_)
58  ROS_WARN("TebVisualization already initialized. Reinitalizing...");
59 
60  // set config
61  cfg_ = &cfg;
62 
63  // register topics
64  global_plan_pub_ = nh.advertise<nav_msgs::Path>("global_plan", 1);
65  local_plan_pub_ = nh.advertise<nav_msgs::Path>("local_plan",1);
66  teb_poses_pub_ = nh.advertise<geometry_msgs::PoseArray>("teb_poses", 100);
67  teb_marker_pub_ = nh.advertise<visualization_msgs::Marker>("teb_markers", 1000);
68  feedback_pub_ = nh.advertise<teb_local_planner::FeedbackMsg>("teb_feedback", 10);
69 
70  initialized_ = true;
71 }
72 
73 
74 
75 void TebVisualization::publishGlobalPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan) const
76 {
77  if ( printErrorWhenNotInitialized() ) return;
79 }
80 
81 void TebVisualization::publishLocalPlan(const std::vector<geometry_msgs::PoseStamped>& local_plan) const
82 {
84  return;
86 }
87 
89 {
91  return;
92 
93  // create path msg
94  nav_msgs::Path teb_path;
95  teb_path.header.frame_id = cfg_->map_frame;
96  teb_path.header.stamp = ros::Time::now();
97 
98  // create pose_array (along trajectory)
99  geometry_msgs::PoseArray teb_poses;
100  teb_poses.header.frame_id = teb_path.header.frame_id;
101  teb_poses.header.stamp = teb_path.header.stamp;
102 
103  // fill path msgs with teb configurations
104  for (int i=0; i < teb.sizePoses(); i++)
105  {
106  geometry_msgs::PoseStamped pose;
107  pose.header.frame_id = teb_path.header.frame_id;
108  pose.header.stamp = teb_path.header.stamp;
109  pose.pose.position.x = teb.Pose(i).x();
110  pose.pose.position.y = teb.Pose(i).y();
112  pose.pose.orientation = tf::createQuaternionMsgFromYaw(teb.Pose(i).theta());
113  teb_path.poses.push_back(pose);
114  teb_poses.poses.push_back(pose.pose);
115  }
116  local_plan_pub_.publish(teb_path);
117  teb_poses_pub_.publish(teb_poses);
118 }
119 
120 
121 
122 void TebVisualization::publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns,
123  const std_msgs::ColorRGBA &color)
124 {
126  return;
127 
128  std::vector<visualization_msgs::Marker> markers;
129  robot_model.visualizeRobot(current_pose, markers, color);
130  if (markers.empty())
131  return;
132 
133  int idx = 1000000; // avoid overshadowing by obstacles
134  for (std::vector<visualization_msgs::Marker>::iterator marker_it = markers.begin(); marker_it != markers.end(); ++marker_it, ++idx)
135  {
136  marker_it->header.frame_id = cfg_->map_frame;
137  marker_it->header.stamp = ros::Time::now();
138  marker_it->action = visualization_msgs::Marker::ADD;
139  marker_it->ns = ns;
140  marker_it->id = idx;
141  marker_it->lifetime = ros::Duration(2.0);
142  teb_marker_pub_.publish(*marker_it);
143  }
144 
145 }
146 
148 {
149  publishRobotFootprintModel(current_pose, robot_model, "InfeasibleRobotPoses", toColorMsg(0.5, 0.8, 0.0, 0.0));
150 }
151 
152 
154 {
155  if ( obstacles.empty() || printErrorWhenNotInitialized() )
156  return;
157 
158  // Visualize point obstacles
159  {
160  visualization_msgs::Marker marker;
161  marker.header.frame_id = cfg_->map_frame;
162  marker.header.stamp = ros::Time::now();
163  marker.ns = "PointObstacles";
164  marker.id = 0;
165  marker.type = visualization_msgs::Marker::POINTS;
166  marker.action = visualization_msgs::Marker::ADD;
167  marker.lifetime = ros::Duration(2.0);
168 
169  for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
170  {
171  boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(*obst);
172  if (!pobst)
173  continue;
174 
176  {
177  geometry_msgs::Point point;
178  point.x = pobst->x();
179  point.y = pobst->y();
180  point.z = 0;
181  marker.points.push_back(point);
182  }
183  else // Spatiotemporally point obstacles become a line
184  {
185  marker.type = visualization_msgs::Marker::LINE_LIST;
186  geometry_msgs::Point start;
187  start.x = pobst->x();
188  start.y = pobst->y();
189  start.z = 0;
190  marker.points.push_back(start);
191 
192  geometry_msgs::Point end;
193  double t = 20;
194  Eigen::Vector2d pred;
195  pobst->predictCentroidConstantVelocity(t, pred);
196  end.x = pred[0];
197  end.y = pred[1];
199  marker.points.push_back(end);
200  }
201  }
202 
203  marker.scale.x = 0.1;
204  marker.scale.y = 0.1;
205  marker.color.a = 1.0;
206  marker.color.r = 1.0;
207  marker.color.g = 0.0;
208  marker.color.b = 0.0;
209 
210  teb_marker_pub_.publish( marker );
211  }
212 
213  // Visualize line obstacles
214  {
215  std::size_t idx = 0;
216  for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
217  {
218  boost::shared_ptr<LineObstacle> pobst = boost::dynamic_pointer_cast<LineObstacle>(*obst);
219  if (!pobst)
220  continue;
221 
222  visualization_msgs::Marker marker;
223  marker.header.frame_id = cfg_->map_frame;
224  marker.header.stamp = ros::Time::now();
225  marker.ns = "LineObstacles";
226  marker.id = idx++;
227  marker.type = visualization_msgs::Marker::LINE_STRIP;
228  marker.action = visualization_msgs::Marker::ADD;
229  marker.lifetime = ros::Duration(2.0);
230  geometry_msgs::Point start;
231  start.x = pobst->start().x();
232  start.y = pobst->start().y();
233  start.z = 0;
234  marker.points.push_back(start);
235  geometry_msgs::Point end;
236  end.x = pobst->end().x();
237  end.y = pobst->end().y();
238  end.z = 0;
239  marker.points.push_back(end);
240 
241  marker.scale.x = 0.1;
242  marker.scale.y = 0.1;
243  marker.color.a = 1.0;
244  marker.color.r = 0.0;
245  marker.color.g = 1.0;
246  marker.color.b = 0.0;
247 
248  teb_marker_pub_.publish( marker );
249  }
250  }
251 
252 
253  // Visualize polygon obstacles
254  {
255  std::size_t idx = 0;
256  for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
257  {
258  boost::shared_ptr<PolygonObstacle> pobst = boost::dynamic_pointer_cast<PolygonObstacle>(*obst);
259  if (!pobst)
260  continue;
261 
262  visualization_msgs::Marker marker;
263  marker.header.frame_id = cfg_->map_frame;
264  marker.header.stamp = ros::Time::now();
265  marker.ns = "PolyObstacles";
266  marker.id = idx++;
267  marker.type = visualization_msgs::Marker::LINE_STRIP;
268  marker.action = visualization_msgs::Marker::ADD;
269  marker.lifetime = ros::Duration(2.0);
270 
271  for (Point2dContainer::const_iterator vertex = pobst->vertices().begin(); vertex != pobst->vertices().end(); ++vertex)
272  {
273  geometry_msgs::Point point;
274  point.x = vertex->x();
275  point.y = vertex->y();
276  point.z = 0;
277  marker.points.push_back(point);
278  }
279 
280  // Also add last point to close the polygon
281  // but only if polygon has more than 2 points (it is not a line)
282  if (pobst->vertices().size() > 2)
283  {
284  geometry_msgs::Point point;
285  point.x = pobst->vertices().front().x();
286  point.y = pobst->vertices().front().y();
287  point.z = 0;
288  marker.points.push_back(point);
289  }
290  marker.scale.x = 0.1;
291  marker.scale.y = 0.1;
292  marker.color.a = 1.0;
293  marker.color.r = 1.0;
294  marker.color.g = 0.0;
295  marker.color.b = 0.0;
296 
297  teb_marker_pub_.publish( marker );
298  }
299  }
300 }
301 
302 
303 void TebVisualization::publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns) const
304 {
305  if ( via_points.empty() || printErrorWhenNotInitialized() )
306  return;
307 
308  visualization_msgs::Marker marker;
309  marker.header.frame_id = cfg_->map_frame;
310  marker.header.stamp = ros::Time::now();
311  marker.ns = ns;
312  marker.id = 0;
313  marker.type = visualization_msgs::Marker::POINTS;
314  marker.action = visualization_msgs::Marker::ADD;
315  marker.lifetime = ros::Duration(2.0);
316 
317  for (std::size_t i=0; i < via_points.size(); ++i)
318  {
319  geometry_msgs::Point point;
320  point.x = via_points[i].x();
321  point.y = via_points[i].y();
322  point.z = 0;
323  marker.points.push_back(point);
324  }
325 
326  marker.scale.x = 0.1;
327  marker.scale.y = 0.1;
328  marker.color.a = 1.0;
329  marker.color.r = 0.0;
330  marker.color.g = 0.0;
331  marker.color.b = 1.0;
332 
333  teb_marker_pub_.publish( marker );
334 }
335 
336 void TebVisualization::publishTebContainer(const TebOptPlannerContainer& teb_planner, const std::string& ns)
337 {
339  return;
340 
341  visualization_msgs::Marker marker;
342  marker.header.frame_id = cfg_->map_frame;
343  marker.header.stamp = ros::Time::now();
344  marker.ns = ns;
345  marker.id = 0;
346  marker.type = visualization_msgs::Marker::LINE_LIST;
347  marker.action = visualization_msgs::Marker::ADD;
348 
349  // Iterate through teb pose sequence
350  for( TebOptPlannerContainer::const_iterator it_teb = teb_planner.begin(); it_teb != teb_planner.end(); ++it_teb )
351  {
352  // iterate single poses
353  PoseSequence::const_iterator it_pose = it_teb->get()->teb().poses().begin();
354  TimeDiffSequence::const_iterator it_timediff = it_teb->get()->teb().timediffs().begin();
355  PoseSequence::const_iterator it_pose_end = it_teb->get()->teb().poses().end();
356  std::advance(it_pose_end, -1); // since we are interested in line segments, reduce end iterator by one.
357  double time = 0;
358 
359  while (it_pose != it_pose_end)
360  {
361  geometry_msgs::Point point_start;
362  point_start.x = (*it_pose)->x();
363  point_start.y = (*it_pose)->y();
364  point_start.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time;
365  marker.points.push_back(point_start);
366 
367  time += (*it_timediff)->dt();
368 
369  geometry_msgs::Point point_end;
370  point_end.x = (*boost::next(it_pose))->x();
371  point_end.y = (*boost::next(it_pose))->y();
372  point_end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time;
373  marker.points.push_back(point_end);
374  ++it_pose;
375  ++it_timediff;
376  }
377  }
378  marker.scale.x = 0.01;
379  marker.color.a = 1.0;
380  marker.color.r = 0.5;
381  marker.color.g = 1.0;
382  marker.color.b = 0.0;
383 
384  teb_marker_pub_.publish( marker );
385 }
386 
388  unsigned int selected_trajectory_idx, const ObstContainer& obstacles)
389 {
390  FeedbackMsg msg;
391  msg.header.stamp = ros::Time::now();
392  msg.header.frame_id = cfg_->map_frame;
393  msg.selected_trajectory_idx = selected_trajectory_idx;
394 
395 
396  msg.trajectories.resize(teb_planners.size());
397 
398  // Iterate through teb pose sequence
399  std::size_t idx_traj = 0;
400  for( TebOptPlannerContainer::const_iterator it_teb = teb_planners.begin(); it_teb != teb_planners.end(); ++it_teb, ++idx_traj )
401  {
402  msg.trajectories[idx_traj].header = msg.header;
403  it_teb->get()->getFullTrajectory(msg.trajectories[idx_traj].trajectory);
404  }
405 
406  // add obstacles
407  msg.obstacles_msg.obstacles.resize(obstacles.size());
408  for (std::size_t i=0; i<obstacles.size(); ++i)
409  {
410  msg.obstacles_msg.header = msg.header;
411 
412  // copy polygon
413  msg.obstacles_msg.obstacles[i].header = msg.header;
414  obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
415 
416  // copy id
417  msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
418 
419  // orientation
420  //msg.obstacles_msg.obstacles[i].orientation =; // TODO
421 
422  // copy velocities
423  obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
424  }
425 
426  feedback_pub_.publish(msg);
427 }
428 
430 {
431  FeedbackMsg msg;
432  msg.header.stamp = ros::Time::now();
433  msg.header.frame_id = cfg_->map_frame;
434  msg.selected_trajectory_idx = 0;
435 
436  msg.trajectories.resize(1);
437  msg.trajectories.front().header = msg.header;
438  teb_planner.getFullTrajectory(msg.trajectories.front().trajectory);
439 
440  // add obstacles
441  msg.obstacles_msg.obstacles.resize(obstacles.size());
442  for (std::size_t i=0; i<obstacles.size(); ++i)
443  {
444  msg.obstacles_msg.header = msg.header;
445 
446  // copy polygon
447  msg.obstacles_msg.obstacles[i].header = msg.header;
448  obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
449 
450  // copy id
451  msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet
452 
453  // orientation
454  //msg.obstacles_msg.obstacles[i].orientation =; // TODO
455 
456  // copy velocities
457  obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
458  }
459 
460  feedback_pub_.publish(msg);
461 }
462 
463 std_msgs::ColorRGBA TebVisualization::toColorMsg(double a, double r, double g, double b)
464 {
465  std_msgs::ColorRGBA color;
466  color.a = a;
467  color.r = r;
468  color.g = g;
469  color.b = b;
470  return color;
471 }
472 
474 {
475  if (!initialized_)
476  {
477  ROS_ERROR("TebVisualization class not initialized. You must call initialize or an appropriate constructor");
478  return true;
479  }
480  return false;
481 }
482 
483 } // namespace teb_local_planner
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.
std::string map_frame
Global planning frame.
Definition: teb_config.h:66
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 publish(const boost::shared_ptr< M > &message) const
ros::Publisher teb_poses_pub_
Publisher for the trajectory pose sequence.
static std_msgs::ColorRGBA toColorMsg(double a, double r, double g, double b)
Helper function to generate a color message from single values.
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
virtual void visualizeRobot(const PoseSE2 &current_pose, std::vector< visualization_msgs::Marker > &markers, const std_msgs::ColorRGBA &color) const
Visualize the robot using a markers.
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...
double & x()
Access the x-coordinate the pose.
Definition: pose_se2.h:158
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
Implements a polygon obstacle with an arbitrary number of vertices.
Definition: obstacles.h:748
double visualize_with_time_as_z_axis_scale
If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as t...
Definition: teb_config.h:194
Implements a 2D line obstacle.
Definition: obstacles.h:597
void publishRobotFootprintModel(const PoseSE2 &current_pose, const BaseRobotFootprintModel &robot_model, const std::string &ns="RobotFootprintModel", const std_msgs::ColorRGBA &color=toColorMsg(0.5, 0.0, 0.8, 0.0))
Publish the visualization of the robot model.
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).
struct teb_local_planner::TebConfig::HomotopyClasses hcp
Implements a 2D point obstacle.
Definition: obstacles.h:305
std::vector< ObstaclePtr > ObstContainer
Abbrev. for containers storing multiple obstacles.
Definition: obstacles.h:297
TFSIMD_FORCE_INLINE const tfScalar & x() const
void getFullTrajectory(std::vector< TrajectoryPointMsg > &trajectory) const
Return the complete trajectory including poses, velocity profiles and temporal information.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher teb_marker_pub_
Publisher for visualization markers.
std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer
Abbrev. for containers storing multiple teb optimal planners.
PoseSE2 & Pose(int index)
Access the pose at pos index of the pose sequence.
double & y()
Access the y-coordinate the pose.
Definition: pose_se2.h:170
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
double & theta()
Access the orientation part (yaw angle) of the pose.
Definition: pose_se2.h:182
TebVisualization()
Default constructor.
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, const ros::Publisher &pub)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
ros::Publisher global_plan_pub_
Publisher for the global plan.
This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework.
void publishInfeasibleRobotPose(const PoseSE2 &current_pose, const BaseRobotFootprintModel &robot_model)
Publish the robot footprints related to infeasible poses.
static Time now()
double getSumOfTimeDiffsUpToIdx(int index) const
Calculate the estimated transition time up to the pose denoted by index.
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)
int sizePoses() const
Get the length of the internal pose sequence.
#define ROS_ERROR(...)


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