trajectory_visualization.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman */
00036 
00037 #include <moveit/rviz_plugin_render_tools/trajectory_visualization.h>
00038 
00039 #include <moveit/rviz_plugin_render_tools/planning_link_updater.h>
00040 #include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
00041 #include <rviz/robot/robot.h>
00042 
00043 #include <rviz/properties/property.h>
00044 #include <rviz/properties/int_property.h>
00045 #include <rviz/properties/string_property.h>
00046 #include <rviz/properties/bool_property.h>
00047 #include <rviz/properties/float_property.h>
00048 #include <rviz/properties/ros_topic_property.h>
00049 #include <rviz/properties/editable_enum_property.h>
00050 #include <rviz/properties/color_property.h>
00051 #include <moveit/trajectory_processing/trajectory_tools.h>
00052 #include <rviz/robot/robot_link.h>
00053 #include <rviz/display_context.h>
00054 #include <rviz/window_manager_interface.h>
00055 
00056 namespace moveit_rviz_plugin
00057 {
00058 TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::Display* display)
00059   : display_(display)
00060   , widget_(widget)
00061   , animating_path_(false)
00062   , drop_displaying_trajectory_(false)
00063   , current_state_(-1)
00064   , trajectory_slider_panel_(NULL)
00065   , trajectory_slider_dock_panel_(NULL)
00066 {
00067   trajectory_topic_property_ =
00068       new rviz::RosTopicProperty("Trajectory Topic", "/move_group/display_planned_path",
00069                                  ros::message_traits::datatype<moveit_msgs::DisplayTrajectory>(),
00070                                  "The topic on which the moveit_msgs::DisplayTrajectory messages are received", widget,
00071                                  SLOT(changedTrajectoryTopic()), this);
00072 
00073   display_path_visual_enabled_property_ =
00074       new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for "
00075                                                         "visualisation purposes should be displayed",
00076                              widget, SLOT(changedDisplayPathVisualEnabled()), this);
00077 
00078   display_path_collision_enabled_property_ =
00079       new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined "
00080                                                             "for collision detection purposes should be displayed",
00081                              widget, SLOT(changedDisplayPathCollisionEnabled()), this);
00082 
00083   robot_path_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links",
00084                                                        widget, SLOT(changedRobotPathAlpha()), this);
00085   robot_path_alpha_property_->setMin(0.0);
00086   robot_path_alpha_property_->setMax(1.0);
00087 
00088   state_display_time_property_ = new rviz::EditableEnumProperty("State Display Time", "0.05 s",
00089                                                                 "The amount of wall-time to wait in between displaying "
00090                                                                 "states along a received trajectory path",
00091                                                                 widget, SLOT(changedStateDisplayTime()), this);
00092   state_display_time_property_->addOptionStd("REALTIME");
00093   state_display_time_property_->addOptionStd("0.05 s");
00094   state_display_time_property_->addOptionStd("0.1 s");
00095   state_display_time_property_->addOptionStd("0.5 s");
00096 
00097   loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path "
00098                                                                            "is to be animated in a loop",
00099                                                   widget, SLOT(changedLoopDisplay()), this);
00100 
00101   trail_display_property_ =
00102       new rviz::BoolProperty("Show Trail", false, "Show a path trail", widget, SLOT(changedShowTrail()), this);
00103 
00104   trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, "Specifies the step size of the samples "
00105                                                                           "shown in the trajectory trail.",
00106                                                     widget, SLOT(changedTrailStepSize()), this);
00107   trail_step_size_property_->setMin(1);
00108 
00109   interrupt_display_property_ = new rviz::BoolProperty(
00110       "Interrupt Display", false,
00111       "Immediately show newly planned trajectory, interrupting the currently displayed one.", widget);
00112 
00113   robot_color_property_ = new rviz::ColorProperty(
00114       "Robot Color", QColor(150, 50, 150), "The color of the animated robot", widget, SLOT(changedRobotColor()), this);
00115 
00116   enable_robot_color_property_ = new rviz::BoolProperty(
00117       "Color Enabled", false, "Specifies whether robot coloring is enabled", widget, SLOT(enabledRobotColor()), this);
00118 }
00119 
00120 TrajectoryVisualization::~TrajectoryVisualization()
00121 {
00122   clearTrajectoryTrail();
00123   trajectory_message_to_display_.reset();
00124   displaying_trajectory_message_.reset();
00125 
00126   display_path_robot_.reset();
00127   if (trajectory_slider_dock_panel_)
00128     delete trajectory_slider_dock_panel_;
00129 }
00130 
00131 void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context,
00132                                            ros::NodeHandle update_nh)
00133 {
00134   // Save pointers for later use
00135   scene_node_ = scene_node;
00136   context_ = context;
00137   update_nh_ = update_nh;
00138 
00139   // Load trajectory robot
00140   display_path_robot_.reset(new RobotStateVisualization(scene_node_, context_, "Planned Path", widget_));
00141   display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
00142   display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
00143   display_path_robot_->setVisible(false);
00144 
00145   rviz::WindowManagerInterface* window_context = context_->getWindowManager();
00146   if (window_context)
00147   {
00148     trajectory_slider_panel_ = new TrajectoryPanel(window_context->getParentWindow());
00149     trajectory_slider_dock_panel_ =
00150         window_context->addPane(display_->getName() + " - Slider", trajectory_slider_panel_);
00151     trajectory_slider_dock_panel_->setIcon(display_->getIcon());
00152     connect(trajectory_slider_dock_panel_, SIGNAL(visibilityChanged(bool)), this,
00153             SLOT(trajectorySliderPanelVisibilityChange(bool)));
00154     trajectory_slider_panel_->onInitialize();
00155   }
00156 }
00157 
00158 void TrajectoryVisualization::setName(const QString& name)
00159 {
00160   if (trajectory_slider_dock_panel_)
00161     trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider");
00162 }
00163 
00164 void TrajectoryVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model)
00165 {
00166   robot_model_ = robot_model;
00167 
00168   // Error check
00169   if (!robot_model_)
00170   {
00171     ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot model found");
00172     return;
00173   }
00174 
00175   // Load robot state
00176   robot_state_.reset(new robot_state::RobotState(robot_model_));
00177   robot_state_->setToDefaultValues();
00178 
00179   // Load rviz robot
00180   display_path_robot_->load(*robot_model_->getURDF());
00181   enabledRobotColor();  // force-refresh to account for saved display configuration
00182 }
00183 
00184 void TrajectoryVisualization::reset()
00185 {
00186   clearTrajectoryTrail();
00187   trajectory_message_to_display_.reset();
00188   displaying_trajectory_message_.reset();
00189   animating_path_ = false;
00190 
00191   display_path_robot_->clear();
00192   display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
00193   display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
00194   display_path_robot_->setVisible(false);
00195 
00196   if (!robot_model_)
00197     ROS_WARN_STREAM_NAMED("trajectory_visualization", "No robot model found");
00198   else
00199     display_path_robot_->load(*robot_model_->getURDF());
00200 }
00201 
00202 void TrajectoryVisualization::clearTrajectoryTrail()
00203 {
00204   for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
00205     delete trajectory_trail_[i];
00206   trajectory_trail_.clear();
00207 }
00208 
00209 void TrajectoryVisualization::changedLoopDisplay()
00210 {
00211   display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
00212   if (loop_display_property_->getBool() && trajectory_slider_panel_)
00213     trajectory_slider_panel_->pauseButton(false);
00214 }
00215 
00216 void TrajectoryVisualization::changedShowTrail()
00217 {
00218   clearTrajectoryTrail();
00219 
00220   if (!trail_display_property_->getBool())
00221     return;
00222   robot_trajectory::RobotTrajectoryPtr t = trajectory_message_to_display_;
00223   if (!t)
00224     t = displaying_trajectory_message_;
00225   if (!t)
00226     return;
00227 
00228   int stepsize = trail_step_size_property_->getInt();
00229   // always include last trajectory point
00230   trajectory_trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize));
00231   for (std::size_t i = 0; i < trajectory_trail_.size(); i++)
00232   {
00233     int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1);  // limit to last trajectory point
00234     rviz::Robot* r = new rviz::Robot(scene_node_, context_, "Trail Robot " + boost::lexical_cast<std::string>(i), NULL);
00235     r->load(*robot_model_->getURDF());
00236     r->setVisualVisible(display_path_visual_enabled_property_->getBool());
00237     r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
00238     r->setAlpha(robot_path_alpha_property_->getFloat());
00239     r->update(PlanningLinkUpdater(t->getWayPointPtr(waypoint_i)));
00240     if (enable_robot_color_property_->getBool())
00241       setRobotColor(r, robot_color_property_->getColor());
00242     r->setVisible(display_->isEnabled() && (!animating_path_ || waypoint_i <= current_state_));
00243     trajectory_trail_[i] = r;
00244   }
00245 }
00246 
00247 void TrajectoryVisualization::changedTrailStepSize()
00248 {
00249   if (trail_display_property_->getBool())
00250     changedShowTrail();
00251 }
00252 
00253 void TrajectoryVisualization::changedRobotPathAlpha()
00254 {
00255   display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat());
00256   for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
00257     trajectory_trail_[i]->setAlpha(robot_path_alpha_property_->getFloat());
00258 }
00259 
00260 void TrajectoryVisualization::changedTrajectoryTopic()
00261 {
00262   trajectory_topic_sub_.shutdown();
00263   if (!trajectory_topic_property_->getStdString().empty())
00264   {
00265     trajectory_topic_sub_ = update_nh_.subscribe(trajectory_topic_property_->getStdString(), 2,
00266                                                  &TrajectoryVisualization::incomingDisplayTrajectory, this);
00267   }
00268 }
00269 
00270 void TrajectoryVisualization::changedDisplayPathVisualEnabled()
00271 {
00272   if (display_->isEnabled())
00273   {
00274     display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
00275     display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
00276     for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
00277       trajectory_trail_[i]->setVisualVisible(display_path_visual_enabled_property_->getBool());
00278   }
00279 }
00280 
00281 void TrajectoryVisualization::changedStateDisplayTime()
00282 {
00283 }
00284 
00285 void TrajectoryVisualization::changedDisplayPathCollisionEnabled()
00286 {
00287   if (display_->isEnabled())
00288   {
00289     display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
00290     display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
00291     for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
00292       trajectory_trail_[i]->setCollisionVisible(display_path_collision_enabled_property_->getBool());
00293   }
00294 }
00295 
00296 void TrajectoryVisualization::onEnable()
00297 {
00298   changedRobotPathAlpha();  // set alpha property
00299 
00300   display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
00301   display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
00302   display_path_robot_->setVisible(displaying_trajectory_message_ && animating_path_);
00303   for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
00304   {
00305     trajectory_trail_[i]->setVisualVisible(display_path_visual_enabled_property_->getBool());
00306     trajectory_trail_[i]->setCollisionVisible(display_path_collision_enabled_property_->getBool());
00307     trajectory_trail_[i]->setVisible(true);
00308   }
00309 
00310   changedTrajectoryTopic();  // load topic at startup if default used
00311 }
00312 
00313 void TrajectoryVisualization::onDisable()
00314 {
00315   display_path_robot_->setVisible(false);
00316   for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
00317     trajectory_trail_[i]->setVisible(false);
00318   displaying_trajectory_message_.reset();
00319   animating_path_ = false;
00320   if (trajectory_slider_panel_)
00321     trajectory_slider_panel_->onDisable();
00322 }
00323 
00324 void TrajectoryVisualization::interruptCurrentDisplay()
00325 {
00326   // update() starts a new trajectory as soon as it is available
00327   // interrupting may cause the newly received trajectory to interrupt
00328   // hence, only interrupt when current_state_ already advanced past first
00329   if (current_state_ > 0)
00330     animating_path_ = false;
00331 }
00332 
00333 float TrajectoryVisualization::getStateDisplayTime()
00334 {
00335   std::string tm = state_display_time_property_->getStdString();
00336   if (tm == "REALTIME")
00337     return -1.0;
00338   else
00339   {
00340     boost::replace_all(tm, "s", "");
00341     boost::trim(tm);
00342     float t = 0.05f;
00343     try
00344     {
00345       t = boost::lexical_cast<float>(tm);
00346     }
00347     catch (const boost::bad_lexical_cast& ex)
00348     {
00349       state_display_time_property_->setStdString("0.05 s");
00350     }
00351     return t;
00352   }
00353 }
00354 
00355 void TrajectoryVisualization::dropTrajectory()
00356 {
00357   drop_displaying_trajectory_ = true;
00358 }
00359 
00360 void TrajectoryVisualization::update(float wall_dt, float ros_dt)
00361 {
00362   if (drop_displaying_trajectory_)
00363   {
00364     animating_path_ = false;
00365     displaying_trajectory_message_.reset();
00366     display_path_robot_->setVisible(false);
00367     trajectory_slider_panel_->update(0);
00368     drop_displaying_trajectory_ = false;
00369   }
00370   if (!animating_path_)
00371   {  // finished last animation?
00372     boost::mutex::scoped_lock lock(update_trajectory_message_);
00373 
00374     // new trajectory available to display?
00375     if (trajectory_message_to_display_ && !trajectory_message_to_display_->empty())
00376     {
00377       animating_path_ = true;
00378       displaying_trajectory_message_ = trajectory_message_to_display_;
00379       changedShowTrail();
00380       if (trajectory_slider_panel_)
00381         trajectory_slider_panel_->update(trajectory_message_to_display_->getWayPointCount());
00382     }
00383     else if (displaying_trajectory_message_)
00384     {
00385       if (loop_display_property_->getBool())
00386       {  // do loop? -> start over too
00387         animating_path_ = true;
00388       }
00389       else if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible())
00390       {
00391         if (trajectory_slider_panel_->getSliderPosition() == displaying_trajectory_message_->getWayPointCount() - 1)
00392         {  // show the last waypoint if the slider is enabled
00393           display_path_robot_->update(
00394               displaying_trajectory_message_->getWayPointPtr(displaying_trajectory_message_->getWayPointCount() - 1));
00395         }
00396         else
00397           animating_path_ = true;
00398       }
00399     }
00400     trajectory_message_to_display_.reset();
00401 
00402     if (animating_path_)
00403     {
00404       current_state_ = -1;
00405       current_state_time_ = std::numeric_limits<float>::infinity();
00406       display_path_robot_->update(displaying_trajectory_message_->getFirstWayPointPtr());
00407       display_path_robot_->setVisible(display_->isEnabled());
00408       if (trajectory_slider_panel_)
00409         trajectory_slider_panel_->setSliderPosition(0);
00410     }
00411   }
00412 
00413   if (animating_path_)
00414   {
00415     float tm = getStateDisplayTime();
00416     if (tm < 0.0)  // if we should use realtime
00417       tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1);
00418     if (current_state_time_ > tm)
00419     {
00420       if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible() && trajectory_slider_panel_->isPaused())
00421         current_state_ = trajectory_slider_panel_->getSliderPosition();
00422       else
00423         ++current_state_;
00424       int waypoint_count = displaying_trajectory_message_->getWayPointCount();
00425       if ((std::size_t)current_state_ < waypoint_count)
00426       {
00427         if (trajectory_slider_panel_)
00428           trajectory_slider_panel_->setSliderPosition(current_state_);
00429         display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(current_state_));
00430         for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
00431           trajectory_trail_[i]->setVisible(
00432               std::min(waypoint_count - 1, static_cast<int>(i) * trail_step_size_property_->getInt()) <=
00433               current_state_);
00434       }
00435       else
00436       {
00437         animating_path_ = false;  // animation finished
00438         display_path_robot_->setVisible(loop_display_property_->getBool());
00439         if (!loop_display_property_->getBool() && trajectory_slider_panel_)
00440           trajectory_slider_panel_->pauseButton(true);
00441       }
00442       current_state_time_ = 0.0f;
00443     }
00444     current_state_time_ += wall_dt;
00445   }
00446 }
00447 
00448 void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg)
00449 {
00450   // Error check
00451   if (!robot_state_ || !robot_model_)
00452   {
00453     ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot state or robot model loaded");
00454     return;
00455   }
00456 
00457   if (!msg->model_id.empty() && msg->model_id != robot_model_->getName())
00458     ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", msg->model_id.c_str(),
00459              robot_model_->getName().c_str());
00460 
00461   trajectory_message_to_display_.reset();
00462 
00463   robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, ""));
00464   for (std::size_t i = 0; i < msg->trajectory.size(); ++i)
00465   {
00466     if (t->empty())
00467     {
00468       t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]);
00469     }
00470     else
00471     {
00472       robot_trajectory::RobotTrajectory tmp(robot_model_, "");
00473       tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->trajectory[i]);
00474       t->append(tmp, 0.0);
00475     }
00476   }
00477 
00478   if (!t->empty())
00479   {
00480     boost::mutex::scoped_lock lock(update_trajectory_message_);
00481     trajectory_message_to_display_.swap(t);
00482     if (interrupt_display_property_->getBool())
00483       interruptCurrentDisplay();
00484   }
00485 }
00486 
00487 void TrajectoryVisualization::changedRobotColor()
00488 {
00489   if (enable_robot_color_property_->getBool())
00490     setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor());
00491 }
00492 
00493 void TrajectoryVisualization::enabledRobotColor()
00494 {
00495   if (enable_robot_color_property_->getBool())
00496     setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor());
00497   else
00498     unsetRobotColor(&(display_path_robot_->getRobot()));
00499 }
00500 
00501 void TrajectoryVisualization::unsetRobotColor(rviz::Robot* robot)
00502 {
00503   typedef rviz::Robot::M_NameToLink LinksMap;
00504   const LinksMap& links = robot->getLinks();
00505   for (LinksMap::const_iterator it = links.begin(); it != links.end(); it++)
00506     it->second->unsetColor();
00507 }
00508 
00509 void TrajectoryVisualization::setRobotColor(rviz::Robot* robot, const QColor& color)
00510 {
00511   typedef rviz::Robot::M_NameToLink LinksMap;
00512   const LinksMap& links = robot->getLinks();
00513   for (LinksMap::const_iterator it = links.begin(); it != links.end(); it++)
00514     it->second->setColor(color.redF(), color.greenF(), color.blueF());
00515 }
00516 
00517 void TrajectoryVisualization::trajectorySliderPanelVisibilityChange(bool enable)
00518 {
00519   if (!trajectory_slider_panel_)
00520     return;
00521 
00522   if (enable)
00523     trajectory_slider_panel_->onEnable();
00524   else
00525     trajectory_slider_panel_->onDisable();
00526 }
00527 
00528 }  // namespace moveit_rviz_plugin


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:22:14