trajectory_visualization.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman */
36 
37 #include <boost/algorithm/string.hpp>
38 #include <boost/algorithm/string/replace.hpp>
39 
41 
44 #include <rviz/robot/robot.h>
45 
55 #include <rviz/robot/robot_link.h>
56 #include <rviz/display_context.h>
58 
59 namespace moveit_rviz_plugin
60 {
62  : animating_path_(false)
63  , drop_displaying_trajectory_(false)
64  , current_state_(-1)
65  , display_(display)
66  , widget_(widget)
67  , trajectory_slider_panel_(NULL)
68  , trajectory_slider_dock_panel_(NULL)
69 {
71  new rviz::RosTopicProperty("Trajectory Topic", "/move_group/display_planned_path",
72  ros::message_traits::datatype<moveit_msgs::DisplayTrajectory>(),
73  "The topic on which the moveit_msgs::DisplayTrajectory messages are received", widget,
74  SLOT(changedTrajectoryTopic()), this);
75 
77  new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for "
78  "visualisation purposes should be displayed",
79  widget, SLOT(changedDisplayPathVisualEnabled()), this);
80 
82  new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined "
83  "for collision detection purposes should be displayed",
84  widget, SLOT(changedDisplayPathCollisionEnabled()), this);
85 
86  robot_path_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links",
87  widget, SLOT(changedRobotPathAlpha()), this);
90 
91  state_display_time_property_ = new rviz::EditableEnumProperty("State Display Time", "0.05 s",
92  "The amount of wall-time to wait in between displaying "
93  "states along a received trajectory path",
94  widget, SLOT(changedStateDisplayTime()), this);
99 
100  loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path "
101  "is to be animated in a loop",
102  widget, SLOT(changedLoopDisplay()), this);
103 
105  new rviz::BoolProperty("Show Trail", false, "Show a path trail", widget, SLOT(changedShowTrail()), this);
106 
107  trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, "Specifies the step size of the samples "
108  "shown in the trajectory trail.",
109  widget, SLOT(changedTrailStepSize()), this);
111 
113  "Interrupt Display", false,
114  "Immediately show newly planned trajectory, interrupting the currently displayed one.", widget);
115 
117  "Robot Color", QColor(150, 50, 150), "The color of the animated robot", widget, SLOT(changedRobotColor()), this);
118 
120  "Color Enabled", false, "Specifies whether robot coloring is enabled", widget, SLOT(enabledRobotColor()), this);
121 }
122 
124 {
128 
129  display_path_robot_.reset();
132 }
133 
134 void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context,
135  ros::NodeHandle update_nh)
136 {
137  // Save pointers for later use
138  scene_node_ = scene_node;
139  context_ = context;
140  update_nh_ = update_nh;
141 
142  // Load trajectory robot
146  display_path_robot_->setVisible(false);
147 
149  if (window_context)
150  {
153  window_context->addPane(display_->getName() + " - Trajectory Slider", trajectory_slider_panel_);
154  trajectory_slider_dock_panel_->setIcon(display_->getIcon());
155  connect(trajectory_slider_dock_panel_, SIGNAL(visibilityChanged(bool)), this,
158  }
159 }
160 
161 void TrajectoryVisualization::setName(const QString& name)
162 {
164  trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider");
165 }
166 
167 void TrajectoryVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model)
168 {
169  robot_model_ = robot_model;
170 
171  // Error check
172  if (!robot_model_)
173  {
174  ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot model found");
175  return;
176  }
177 
178  // Load robot state
179  robot_state_.reset(new robot_state::RobotState(robot_model_));
180  robot_state_->setToDefaultValues();
181 
182  // Load rviz robot
183  display_path_robot_->load(*robot_model_->getURDF());
184  enabledRobotColor(); // force-refresh to account for saved display configuration
185 }
186 
188 {
192  animating_path_ = false;
193 
196  display_path_robot_->setVisible(false);
197 }
198 
200 {
201  for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
202  delete trajectory_trail_[i];
203  trajectory_trail_.clear();
204 }
205 
207 {
211 }
212 
214 {
216 
218  return;
219  robot_trajectory::RobotTrajectoryPtr t = trajectory_message_to_display_;
220  if (!t)
222  if (!t)
223  return;
224 
225  int stepsize = trail_step_size_property_->getInt();
226  // always include last trajectory point
227  trajectory_trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize));
228  for (std::size_t i = 0; i < trajectory_trail_.size(); i++)
229  {
230  int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point
231  rviz::Robot* r = new rviz::Robot(scene_node_, context_, "Trail Robot " + boost::lexical_cast<std::string>(i), NULL);
232  r->load(*robot_model_->getURDF());
233  r->setVisualVisible(display_path_visual_enabled_property_->getBool());
234  r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
235  r->setAlpha(robot_path_alpha_property_->getFloat());
236  r->update(PlanningLinkUpdater(t->getWayPointPtr(waypoint_i)));
239  r->setVisible(display_->isEnabled() && (!animating_path_ || waypoint_i <= current_state_));
240  trajectory_trail_[i] = r;
241  }
242 }
243 
245 {
248 }
249 
251 {
253  for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
255 }
256 
258 {
261  {
264  }
265 }
266 
268 {
269  if (display_->isEnabled())
270  {
273  for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
275  }
276 }
277 
279 {
280 }
281 
283 {
284  if (display_->isEnabled())
285  {
288  for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
290  }
291 }
292 
294 {
295  changedRobotPathAlpha(); // set alpha property
296 
300  for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
301  {
304  trajectory_trail_[i]->setVisible(true);
305  }
306 
307  changedTrajectoryTopic(); // load topic at startup if default used
308 }
309 
311 {
312  display_path_robot_->setVisible(false);
313  for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
314  trajectory_trail_[i]->setVisible(false);
316  animating_path_ = false;
319 }
320 
322 {
323  // update() starts a new trajectory as soon as it is available
324  // interrupting may cause the newly received trajectory to interrupt
325  // hence, only interrupt when current_state_ already advanced past first
326  if (current_state_ > 0)
327  animating_path_ = false;
328 }
329 
331 {
332  std::string tm = state_display_time_property_->getStdString();
333  if (tm == "REALTIME")
334  return -1.0;
335  else
336  {
337  boost::replace_all(tm, "s", "");
338  boost::trim(tm);
339  float t = 0.05f;
340  try
341  {
342  t = boost::lexical_cast<float>(tm);
343  }
344  catch (const boost::bad_lexical_cast& ex)
345  {
347  }
348  return t;
349  }
350 }
351 
353 {
355 }
356 
357 void TrajectoryVisualization::update(float wall_dt, float ros_dt)
358 {
360  {
361  animating_path_ = false;
365  }
366  if (!animating_path_)
367  { // finished last animation?
368  boost::mutex::scoped_lock lock(update_trajectory_message_);
369 
370  // new trajectory available to display?
372  {
373  animating_path_ = true;
378  }
380  {
382  { // do loop? -> start over too
383  animating_path_ = true;
384  }
385  else if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible())
386  {
387  if (static_cast<unsigned int>(trajectory_slider_panel_->getSliderPosition()) >=
388  displaying_trajectory_message_->getWayPointCount() - 1)
389  return; // nothing more to do
390  else
391  animating_path_ = true;
392  }
393  }
395 
396  if (animating_path_)
397  {
398  // restart animation
399  current_state_ = -1;
400  }
401  }
402 
403  if (animating_path_)
404  {
405  int previous_state = current_state_;
406  int waypoint_count = displaying_trajectory_message_->getWayPointCount();
407  current_state_time_ += wall_dt;
408  float tm = getStateDisplayTime();
409 
412  else if (current_state_ < 0)
413  { // special case indicating restart of animation
414  current_state_ = 0;
415  current_state_time_ = 0.0;
416  }
417  else if (tm < 0.0)
418  { // using realtime: skip to next waypoint based on elapsed display time
419  while (current_state_ < waypoint_count &&
420  (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1)) <
422  {
423  current_state_time_ -= tm;
424  ++current_state_;
425  }
426  }
427  else if (current_state_time_ > tm)
428  { // fixed display time per state
429  current_state_time_ = 0.0;
430  ++current_state_;
431  }
432 
433  if (current_state_ == previous_state)
434  return;
435 
436  if (current_state_ < waypoint_count)
437  {
441  for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
442  trajectory_trail_[i]->setVisible(
443  std::min(waypoint_count - 1, static_cast<int>(i) * trail_step_size_property_->getInt()) <= current_state_);
444  }
445  else
446  {
447  animating_path_ = false; // animation finished
451  }
452  }
453  // set visibility
457 }
458 
459 void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg)
460 {
461  // Error check
462  if (!robot_state_ || !robot_model_)
463  {
464  ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot state or robot model loaded");
465  return;
466  }
467 
468  if (!msg->model_id.empty() && msg->model_id != robot_model_->getName())
469  ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", msg->model_id.c_str(),
470  robot_model_->getName().c_str());
471 
473 
474  robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, ""));
475  for (std::size_t i = 0; i < msg->trajectory.size(); ++i)
476  {
477  if (t->empty())
478  {
479  t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]);
480  }
481  else
482  {
484  tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->trajectory[i]);
485  t->append(tmp, 0.0);
486  }
487  }
488 
489  if (!t->empty())
490  {
491  boost::mutex::scoped_lock lock(update_trajectory_message_);
495  }
496 }
497 
499 {
502 }
503 
505 {
508  else
509  unsetRobotColor(&(display_path_robot_->getRobot()));
510 }
511 
513 {
514  for (auto& link : robot->getLinks())
515  link.second->unsetColor();
516 }
517 
519 {
520  if (!display_path_robot_)
521  return;
522 
523  std_msgs::ColorRGBA color_msg;
524  color_msg.r = color.redF();
525  color_msg.g = color.greenF();
526  color_msg.b = color.blueF();
527  color_msg.a = color.alphaF();
528  display_path_robot_->setDefaultAttachedObjectColor(color_msg);
529 }
530 
531 void TrajectoryVisualization::setRobotColor(rviz::Robot* robot, const QColor& color)
532 {
533  for (auto& link : robot->getLinks())
534  robot->getLink(link.first)->setColor(color.redF(), color.greenF(), color.blueF());
535 }
536 
538 {
540  return;
541 
542  if (enable)
544  else
546 }
547 
548 } // namespace moveit_rviz_plugin
virtual QColor getColor() const
#define NULL
void setMin(float min)
void setMax(float max)
#define ROS_ERROR_STREAM_NAMED(name, args)
robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_
Update the links of an rviz::Robot using a robot_state::RobotState.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual int getInt() const
virtual QIcon getIcon() const
f
virtual float getFloat() const
void update(int way_point_count)
virtual QWidget * getParentWindow()=0
void setMin(int min)
void onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model)
void addOptionStd(const std::string &option)
#define ROS_WARN(...)
geometry_msgs::TransformStamped t
void setWindowTitle(QString title)
RobotLink * getLink(const std::string &name)
const M_NameToLink & getLinks() const
std::string getStdString()
bool isEnabled() const
virtual bool getBool() const
void onInitialize(Ogre::SceneNode *scene_node, rviz::DisplayContext *context, ros::NodeHandle update_nh)
virtual PanelDockWidget * addPane(const QString &name, QWidget *pane, Qt::DockWidgetArea area=Qt::LeftDockWidgetArea, bool floating=true)=0
TrajectoryVisualization(rviz::Property *widget, rviz::Display *display)
Playback a trajectory from a planned path.
robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_
void incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr &msg)
ROS callback for an incoming path message.
r
virtual void update(float wall_dt, float ros_dt)
void setRobotColor(rviz::Robot *robot, const QColor &color)
bool setStdString(const std::string &std_str)
void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state, const trajectory_msgs::JointTrajectory &trajectory)
virtual QString getName() const
virtual WindowManagerInterface * getWindowManager() const =0
void changedDisplayPathVisualEnabled()
Slot Event Functions.
Update the links of an rviz::Robot using a robot_state::RobotState.
rviz::EditableEnumProperty * state_display_time_property_


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:19:09