visualization.cpp
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 #include <teb_local_planner/visualization.h>
00040 #include <teb_local_planner/optimal_planner.h>
00041 #include <teb_local_planner/FeedbackMsg.h>
00042 
00043 namespace teb_local_planner
00044 {
00045 
00046 TebVisualization::TebVisualization() : initialized_(false)
00047 {
00048 }
00049 
00050 TebVisualization::TebVisualization(ros::NodeHandle& nh, const TebConfig& cfg) : initialized_(false)
00051 {
00052   initialize(nh, cfg);
00053 }
00054 
00055 void TebVisualization::initialize(ros::NodeHandle& nh, const TebConfig& cfg)
00056 {
00057   if (initialized_)
00058     ROS_WARN("TebVisualization already initialized. Reinitalizing...");
00059   
00060   // set config
00061   cfg_ = &cfg;
00062   
00063   // register topics
00064   global_plan_pub_ = nh.advertise<nav_msgs::Path>("global_plan", 1);
00065   local_plan_pub_ = nh.advertise<nav_msgs::Path>("local_plan",1);
00066   teb_poses_pub_ = nh.advertise<geometry_msgs::PoseArray>("teb_poses", 100);
00067   teb_marker_pub_ = nh.advertise<visualization_msgs::Marker>("teb_markers", 1000);
00068   feedback_pub_ = nh.advertise<teb_local_planner::FeedbackMsg>("teb_feedback", 10);  
00069   
00070   initialized_ = true; 
00071 }
00072 
00073 
00074 
00075 void TebVisualization::publishGlobalPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan) const
00076 {
00077   if ( printErrorWhenNotInitialized() ) return;
00078   base_local_planner::publishPlan(global_plan, global_plan_pub_); 
00079 }
00080 
00081 void TebVisualization::publishLocalPlan(const std::vector<geometry_msgs::PoseStamped>& local_plan) const
00082 {
00083   if ( printErrorWhenNotInitialized() )
00084     return;
00085   base_local_planner::publishPlan(local_plan, local_plan_pub_); 
00086 }
00087 
00088 void TebVisualization::publishLocalPlanAndPoses(const TimedElasticBand& teb) const
00089 {
00090   if ( printErrorWhenNotInitialized() )
00091     return;
00092   
00093     // create path msg
00094     nav_msgs::Path teb_path;
00095     teb_path.header.frame_id = cfg_->map_frame;
00096     teb_path.header.stamp = ros::Time::now();
00097     
00098     // create pose_array (along trajectory)
00099     geometry_msgs::PoseArray teb_poses;
00100     teb_poses.header.frame_id = teb_path.header.frame_id;
00101     teb_poses.header.stamp = teb_path.header.stamp;
00102     
00103     // fill path msgs with teb configurations
00104     for (unsigned int i=0; i < teb.sizePoses(); i++)
00105     {
00106       geometry_msgs::PoseStamped pose;
00107       pose.header.frame_id = teb_path.header.frame_id;
00108       pose.header.stamp = teb_path.header.stamp;
00109       pose.pose.position.x = teb.Pose(i).x();
00110       pose.pose.position.y = teb.Pose(i).y();
00111       pose.pose.position.z = 0;
00112       pose.pose.orientation = tf::createQuaternionMsgFromYaw(teb.Pose(i).theta());
00113       teb_path.poses.push_back(pose);
00114       teb_poses.poses.push_back(pose.pose);
00115     }
00116     local_plan_pub_.publish(teb_path);
00117     teb_poses_pub_.publish(teb_poses);
00118 }
00119 
00120 
00121 
00122 void TebVisualization::publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns)
00123 {
00124   if ( printErrorWhenNotInitialized() )
00125     return;
00126   
00127   std::vector<visualization_msgs::Marker> markers;
00128   robot_model.visualizeRobot(current_pose, markers);
00129   if (markers.empty())
00130     return;
00131   
00132   int idx = 0;
00133   for (std::vector<visualization_msgs::Marker>::iterator marker_it = markers.begin(); marker_it != markers.end(); ++marker_it, ++idx)
00134   {
00135     marker_it->header.frame_id = cfg_->map_frame;
00136     marker_it->header.stamp = ros::Time::now();
00137     marker_it->action = visualization_msgs::Marker::ADD;
00138     marker_it->ns = ns;
00139     marker_it->id = idx;
00140     marker_it->lifetime = ros::Duration(2.0);
00141     teb_marker_pub_.publish(*marker_it);
00142   }
00143   
00144 }
00145 
00146 
00147 void TebVisualization::publishObstacles(const ObstContainer& obstacles) const
00148 {
00149   if ( obstacles.empty() || printErrorWhenNotInitialized() )
00150     return;
00151   
00152   // Visualize point obstacles
00153   {
00154     visualization_msgs::Marker marker;
00155     marker.header.frame_id = cfg_->map_frame;
00156     marker.header.stamp = ros::Time::now();
00157     marker.ns = "PointObstacles";
00158     marker.id = 0;
00159     marker.type = visualization_msgs::Marker::POINTS;
00160     marker.action = visualization_msgs::Marker::ADD;
00161     marker.lifetime = ros::Duration(2.0);
00162     
00163     for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
00164     {
00165       boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(*obst);      
00166       if (!pobst)
00167         continue;
00168       geometry_msgs::Point point;
00169       point.x = pobst->x();
00170       point.y = pobst->y();
00171       point.z = 0;
00172       marker.points.push_back(point);
00173     }
00174     
00175     marker.scale.x = 0.1;
00176     marker.scale.y = 0.1;
00177     marker.color.a = 1.0;
00178     marker.color.r = 1.0;
00179     marker.color.g = 0.0;
00180     marker.color.b = 0.0;
00181 
00182     teb_marker_pub_.publish( marker );
00183   }
00184   
00185   // Visualize line obstacles
00186   {
00187     unsigned int idx = 0;
00188     for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
00189     {   
00190       boost::shared_ptr<LineObstacle> pobst = boost::dynamic_pointer_cast<LineObstacle>(*obst);   
00191       if (!pobst)
00192         continue;
00193       
00194       visualization_msgs::Marker marker;
00195       marker.header.frame_id = cfg_->map_frame;
00196       marker.header.stamp = ros::Time::now();
00197       marker.ns = "LineObstacles";
00198       marker.id = idx++;
00199       marker.type = visualization_msgs::Marker::LINE_STRIP;
00200       marker.action = visualization_msgs::Marker::ADD;
00201       marker.lifetime = ros::Duration(2.0);
00202       geometry_msgs::Point start;
00203       start.x = pobst->start().x();
00204       start.y = pobst->start().y();
00205       start.z = 0;
00206       marker.points.push_back(start);
00207       geometry_msgs::Point end;
00208       end.x = pobst->end().x();
00209       end.y = pobst->end().y();
00210       end.z = 0;
00211       marker.points.push_back(end);
00212   
00213       marker.scale.x = 0.1;
00214       marker.scale.y = 0.1;
00215       marker.color.a = 1.0;
00216       marker.color.r = 0.0;
00217       marker.color.g = 1.0;
00218       marker.color.b = 0.0;
00219       
00220       teb_marker_pub_.publish( marker );     
00221     }
00222   }
00223   
00224 
00225   // Visualize polygon obstacles
00226   {
00227     unsigned int idx = 0;
00228     for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
00229     {   
00230       boost::shared_ptr<PolygonObstacle> pobst = boost::dynamic_pointer_cast<PolygonObstacle>(*obst);   
00231       if (!pobst)
00232                                 continue;
00233       
00234       visualization_msgs::Marker marker;
00235       marker.header.frame_id = cfg_->map_frame;
00236       marker.header.stamp = ros::Time::now();
00237       marker.ns = "PolyObstacles";
00238       marker.id = idx++;
00239       marker.type = visualization_msgs::Marker::LINE_STRIP;
00240       marker.action = visualization_msgs::Marker::ADD;
00241       marker.lifetime = ros::Duration(2.0);
00242       
00243       for (Point2dContainer::const_iterator vertex = pobst->vertices().begin(); vertex != pobst->vertices().end(); ++vertex)
00244       {
00245         geometry_msgs::Point point;
00246         point.x = vertex->x();
00247         point.y = vertex->y();
00248         point.z = 0;
00249         marker.points.push_back(point);
00250       }
00251       
00252       // Also add last point to close the polygon
00253       // but only if polygon has more than 2 points (it is not a line)
00254       if (pobst->vertices().size() > 2)
00255       {
00256         geometry_msgs::Point point;
00257         point.x = pobst->vertices().front().x();
00258         point.y = pobst->vertices().front().y();
00259         point.z = 0;
00260         marker.points.push_back(point);
00261       }
00262       marker.scale.x = 0.1;
00263       marker.scale.y = 0.1;
00264       marker.color.a = 1.0;
00265       marker.color.r = 1.0;
00266       marker.color.g = 0.0;
00267       marker.color.b = 0.0;
00268       
00269       teb_marker_pub_.publish( marker );     
00270     }
00271   }
00272 }
00273 
00274 
00275 void TebVisualization::publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> >& via_points, const std::string& ns) const
00276 {
00277   if ( via_points.empty() || printErrorWhenNotInitialized() )
00278     return;
00279   
00280   visualization_msgs::Marker marker;
00281   marker.header.frame_id = cfg_->map_frame;
00282   marker.header.stamp = ros::Time::now();
00283   marker.ns = ns;
00284   marker.id = 0;
00285   marker.type = visualization_msgs::Marker::POINTS;
00286   marker.action = visualization_msgs::Marker::ADD;
00287   marker.lifetime = ros::Duration(2.0);
00288   
00289   for (std::size_t i=0; i < via_points.size(); ++i)
00290   {
00291     geometry_msgs::Point point;
00292     point.x = via_points[i].x();
00293     point.y = via_points[i].y();
00294     point.z = 0;
00295     marker.points.push_back(point);
00296   }
00297   
00298   marker.scale.x = 0.1;
00299   marker.scale.y = 0.1;
00300   marker.color.a = 1.0;
00301   marker.color.r = 0.0;
00302   marker.color.g = 0.0;
00303   marker.color.b = 1.0;
00304 
00305   teb_marker_pub_.publish( marker );
00306 }
00307 
00308 void TebVisualization::publishTebContainer(const TebOptPlannerContainer& teb_planner, const std::string& ns)
00309 {
00310 if ( printErrorWhenNotInitialized() )
00311     return;
00312   
00313   visualization_msgs::Marker marker;
00314   marker.header.frame_id = cfg_->map_frame;
00315   marker.header.stamp = ros::Time::now();
00316   marker.ns = ns;
00317   marker.id = 0;
00318   marker.type = visualization_msgs::Marker::LINE_LIST;
00319   marker.action = visualization_msgs::Marker::ADD;
00320   
00321   // Iterate through teb pose sequence
00322   for( TebOptPlannerContainer::const_iterator it_teb = teb_planner.begin(); it_teb != teb_planner.end(); ++it_teb )
00323   {       
00324     // iterate single poses
00325     PoseSequence::const_iterator it_pose = it_teb->get()->teb().poses().begin();
00326     PoseSequence::const_iterator it_pose_end = it_teb->get()->teb().poses().end();
00327     std::advance(it_pose_end, -1); // since we are interested in line segments, reduce end iterator by one.
00328     while (it_pose != it_pose_end) 
00329     {
00330       geometry_msgs::Point point_start;
00331       point_start.x = (*it_pose)->x();
00332       point_start.y = (*it_pose)->y();
00333       point_start.z = 0;
00334       marker.points.push_back(point_start);
00335 
00336       geometry_msgs::Point point_end;
00337       point_end.x = (*boost::next(it_pose))->x();
00338       point_end.y = (*boost::next(it_pose))->y();
00339       point_end.z = 0;
00340       marker.points.push_back(point_end);
00341       ++it_pose;
00342     }
00343   }
00344   marker.scale.x = 0.01;
00345   marker.color.a = 1.0;
00346   marker.color.r = 0.5;
00347   marker.color.g = 1.0;
00348   marker.color.b = 0.0;
00349 
00350   teb_marker_pub_.publish( marker );
00351 }
00352 
00353 void TebVisualization::publishFeedbackMessage(const std::vector< boost::shared_ptr<TebOptimalPlanner> >& teb_planners,
00354                                               unsigned int selected_trajectory_idx, const ObstContainer& obstacles)
00355 {
00356   FeedbackMsg msg;
00357   msg.header.stamp = ros::Time::now();
00358   msg.header.frame_id = cfg_->map_frame;
00359   msg.selected_trajectory_idx = selected_trajectory_idx;
00360   
00361   
00362   msg.trajectories.resize(teb_planners.size());
00363   
00364   // Iterate through teb pose sequence
00365   std::size_t idx_traj = 0;
00366   for( TebOptPlannerContainer::const_iterator it_teb = teb_planners.begin(); it_teb != teb_planners.end(); ++it_teb, ++idx_traj )
00367   {   
00368     msg.trajectories[idx_traj].header = msg.header;
00369     it_teb->get()->getFullTrajectory(msg.trajectories[idx_traj].trajectory);
00370   }
00371   
00372   // add obstacles
00373   msg.obstacles.resize(obstacles.size());
00374   for (std::size_t i=0; i<obstacles.size(); ++i)
00375   {
00376     msg.obstacles[i].header = msg.header;
00377     obstacles[i]->toPolygonMsg(msg.obstacles[i].polygon);
00378   }
00379   
00380   feedback_pub_.publish(msg);
00381 }
00382 
00383 void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles)
00384 {
00385   FeedbackMsg msg;
00386   msg.header.stamp = ros::Time::now();
00387   msg.header.frame_id = cfg_->map_frame;
00388   msg.selected_trajectory_idx = 0;
00389   
00390   msg.trajectories.resize(1);
00391   msg.trajectories.front().header = msg.header;
00392   teb_planner.getFullTrajectory(msg.trajectories.front().trajectory);
00393  
00394   // add obstacles
00395   msg.obstacles.resize(obstacles.size());
00396   for (std::size_t i=0; i<obstacles.size(); ++i)
00397   {
00398     msg.obstacles[i].header = msg.header;
00399     obstacles[i]->toPolygonMsg(msg.obstacles[i].polygon);
00400   }
00401   
00402   feedback_pub_.publish(msg);
00403 }
00404 
00405 inline bool TebVisualization::printErrorWhenNotInitialized() const
00406 {
00407   if (!initialized_)
00408   {
00409     ROS_ERROR("TebVisualization class not initialized. You must call initialize or an appropriate constructor");
00410     return true;
00411   }
00412   return false;
00413 }
00414 
00415 } // namespace teb_local_planner


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Mon Oct 24 2016 05:31:15