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 
47 #include <rviz/display_context.h>
56 #include <rviz/robot/robot_link.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_(nullptr)
68  , trajectory_slider_dock_panel_(nullptr)
69 {
70  trajectory_topic_property_ =
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 
76  display_path_visual_enabled_property_ =
77  new rviz::BoolProperty("Show Robot Visual", true,
78  "Indicates whether the geometry of the robot as defined for "
79  "visualisation purposes should be displayed",
80  widget, SLOT(changedDisplayPathVisualEnabled()), this);
81 
82  display_path_collision_enabled_property_ =
83  new rviz::BoolProperty("Show Robot Collision", false,
84  "Indicates whether the geometry of the robot as defined "
85  "for collision detection purposes should be displayed",
86  widget, SLOT(changedDisplayPathCollisionEnabled()), this);
87 
88  robot_path_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links",
89  widget, SLOT(changedRobotPathAlpha()), this);
90  robot_path_alpha_property_->setMin(0.0);
91  robot_path_alpha_property_->setMax(1.0);
92 
93  state_display_time_property_ =
94  new rviz::EditableEnumProperty("State Display Time", "3x",
95  "Replay speed of trajectory. Either as factor of its time"
96  "parameterization or as constant time (s) per waypoint.",
97  widget, SLOT(changedStateDisplayTime()), this);
98  state_display_time_property_->addOptionStd("3x");
99  state_display_time_property_->addOptionStd("1x");
100  state_display_time_property_->addOptionStd("0.5x");
101  state_display_time_property_->addOptionStd("0.05s");
102  state_display_time_property_->addOptionStd("0.1s");
103  state_display_time_property_->addOptionStd("0.5s");
104 
105  use_sim_time_property_ = new rviz::BoolProperty("Use Sim Time", false,
106  "Indicates whether simulation time or wall-time is "
107  "used for state display timing.",
108  widget);
109 
110  loop_display_property_ = new rviz::BoolProperty("Loop Animation", false,
111  "Indicates whether the last received path "
112  "is to be animated in a loop",
113  widget, SLOT(changedLoopDisplay()), this);
114 
115  trail_display_property_ =
116  new rviz::BoolProperty("Show Trail", false, "Show a path trail", widget, SLOT(changedShowTrail()), this);
117 
118  trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1,
119  "Specifies the step size of the samples "
120  "shown in the trajectory trail.",
121  widget, SLOT(changedTrailStepSize()), this);
122  trail_step_size_property_->setMin(1);
123 
124  interrupt_display_property_ =
125  new rviz::BoolProperty("Interrupt Display", false,
126  "Immediately show newly planned trajectory, interrupting the currently displayed one.",
127  widget);
128 
129  robot_color_property_ = new rviz::ColorProperty(
130  "Robot Color", QColor(150, 50, 150), "The color of the animated robot", widget, SLOT(changedRobotColor()), this);
131 
132  enable_robot_color_property_ = new rviz::BoolProperty(
133  "Color Enabled", false, "Specifies whether robot coloring is enabled", widget, SLOT(enabledRobotColor()), this);
134 }
135 
137 {
141 
142  display_path_robot_.reset();
145 }
146 
147 void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context,
148  const ros::NodeHandle& update_nh)
149 {
150  // Save pointers for later use
151  scene_node_ = scene_node;
152  context_ = context;
153  update_nh_ = update_nh;
154 
155  // Load trajectory robot
156  display_path_robot_ = std::make_shared<RobotStateVisualization>(scene_node_, context_, "Planned Path", widget_);
159  display_path_robot_->setVisible(false);
160 
162  if (window_context)
163  {
164  trajectory_slider_panel_ = new TrajectoryPanel(window_context->getParentWindow());
166  window_context->addPane(display_->getName() + " - Trajectory Slider", trajectory_slider_panel_);
168  connect(trajectory_slider_dock_panel_, SIGNAL(visibilityChanged(bool)), this,
171  }
172 }
173 
174 void TrajectoryVisualization::setName(const QString& name)
175 {
177  trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider");
178 }
179 
180 void TrajectoryVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model)
181 {
182  robot_model_ = robot_model;
183 
184  // Error check
185  if (!robot_model_)
186  {
187  ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot model found");
188  return;
189  }
190 
191  // Load robot state
192  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
193  robot_state_->setToDefaultValues();
194 
195  // Load rviz robot
196  display_path_robot_->load(*robot_model_->getURDF());
197  enabledRobotColor(); // force-refresh to account for saved display configuration
198  // perform post-poned subscription to trajectory topic
199  if (trajectory_topic_sub_.getTopic().empty())
201 }
202 
204 {
208  animating_path_ = false;
209 
212  display_path_robot_->setVisible(false);
213 }
214 
216 {
217  trajectory_trail_.clear();
218 }
219 
221 {
225 }
226 
228 {
230 
232  return;
233  robot_trajectory::RobotTrajectoryPtr t = trajectory_message_to_display_;
234  if (!t)
236  if (!t)
237  return;
238 
239  int stepsize = trail_step_size_property_->getInt();
240  // always include last trajectory point
241  trajectory_trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize));
242  for (std::size_t i = 0; i < trajectory_trail_.size(); i++)
243  {
244  int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point
245  auto r = std::make_unique<RobotStateVisualization>(scene_node_, context_,
246  "Trail Robot " + boost::lexical_cast<std::string>(i), nullptr);
247  r->load(*robot_model_->getURDF());
248  r->setVisualVisible(display_path_visual_enabled_property_->getBool());
249  r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
250  r->setAlpha(robot_path_alpha_property_->getFloat());
251  r->update(t->getWayPointPtr(waypoint_i), default_attached_object_color_);
253  setRobotColor(&(r->getRobot()), robot_color_property_->getColor());
254  r->setVisible(display_->isEnabled() && (!animating_path_ || waypoint_i <= current_state_));
255  const auto& robot_path_links = display_path_robot_->getRobot().getLinks();
256  for (const auto& [link_name, robot_link] : robot_path_links)
257  {
258  rviz::RobotLink* l = r->getRobot().getLink(link_name);
259  rviz::Property* link_property = robot_link->getLinkProperty();
260  l->getLinkProperty()->setValue(link_property->getValue());
261  connect(link_property, &rviz::Property::changed, l,
262  [l, link_property]() { l->getLinkProperty()->setValue(link_property->getValue()); });
263  }
264  trajectory_trail_[i] = std::move(r);
265  }
266 }
267 
269 {
272 }
273 
275 {
277  for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
278  r->setAlpha(robot_path_alpha_property_->getFloat());
279 }
280 
282 {
284  // post-pone subscription if robot_state_ is not yet defined, i.e. onRobotModelLoaded() not yet called
286  {
289  }
290 }
291 
293 {
294  if (display_->isEnabled())
295  {
298  for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
299  r->setVisualVisible(display_path_visual_enabled_property_->getBool());
300  }
301 }
302 
304 {
305 }
306 
308 {
309  if (display_->isEnabled())
310  {
313  for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
314  r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
315  }
316 }
317 
319 {
320  changedRobotPathAlpha(); // set alpha property
321 
325  for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
326  {
327  r->setVisualVisible(display_path_visual_enabled_property_->getBool());
328  r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
329  r->setVisible(true);
330  }
331 
332  changedTrajectoryTopic(); // load topic at startup if default used
333 }
334 
336 {
337  display_path_robot_->setVisible(false);
338  for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
339  r->setVisible(false);
341  animating_path_ = false;
344 }
345 
347 {
348  // update() starts a new trajectory as soon as it is available
349  // interrupting may cause the newly received trajectory to interrupt
350  // hence, only interrupt when current_state_ already advanced past first
351  if (current_state_ > 0)
352  animating_path_ = false;
353 }
354 
356 {
357  constexpr char default_time_string[] = "3x";
358  constexpr float default_time_value = -3.0f;
359 
360  std::string tm = state_display_time_property_->getStdString();
361  boost::trim(tm);
362 
363  float type;
364 
365  if (tm.back() == 'x')
366  {
367  type = -1.0f;
368  }
369  else if (tm.back() == 's')
370  {
371  type = 1.0f;
372  }
373  else
374  {
375  state_display_time_property_->setStdString(default_time_string);
376  return default_time_value;
377  }
378 
379  tm.resize(tm.size() - 1);
380  boost::trim_right(tm);
381 
382  float value;
383  try
384  {
385  value = boost::lexical_cast<float>(tm);
386  }
387  catch (const boost::bad_lexical_cast& ex)
388  {
389  state_display_time_property_->setStdString(default_time_string);
390  return default_time_value;
391  }
392 
393  if (value <= 0)
394  {
395  state_display_time_property_->setStdString(default_time_string);
396  return default_time_value;
397  }
398 
399  return type * value;
400 }
401 
403 {
405 }
406 
407 void TrajectoryVisualization::update(float wall_dt, float sim_dt)
408 {
410  {
411  animating_path_ = false;
415  }
416  if (!animating_path_)
417  { // finished last animation?
418  boost::mutex::scoped_lock lock(update_trajectory_message_);
419 
420  // new trajectory available to display?
422  {
423  animating_path_ = true;
428  }
430  {
432  { // do loop? -> start over too
433  animating_path_ = true;
434  }
435  else if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible())
436  {
437  if (static_cast<unsigned int>(trajectory_slider_panel_->getSliderPosition()) >=
438  displaying_trajectory_message_->getWayPointCount() - 1)
439  return; // nothing more to do
440  else
441  animating_path_ = true;
442  }
443  }
445 
446  if (animating_path_)
447  {
448  // restart animation
449  current_state_ = -1;
450  }
451  }
452 
453  if (animating_path_)
454  {
455  int previous_state = current_state_;
456  int waypoint_count = displaying_trajectory_message_->getWayPointCount();
458  {
459  current_state_time_ += sim_dt;
460  }
461  else
462  {
463  current_state_time_ += wall_dt;
464  }
465  float tm = getStateDisplayTime();
466 
468  {
470  current_state_time_ = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_);
471  }
472  else if (current_state_ < 0)
473  { // special case indicating restart of animation
474  current_state_ = 0;
475  current_state_time_ = 0.0;
476  }
477  else if (tm < 0.0)
478  {
479  // using realtime factors: skip to next waypoint based on elapsed display time
480  const float rt_factor = -tm; // negative tm is the intended realtime factor
481  while (current_state_ < waypoint_count &&
482  (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1) / rt_factor) <
484  {
485  current_state_time_ -= tm;
486  // if we are stuck in the while loop we should move the robot along the path to keep up
487  if (tm < current_state_time_)
489  ++current_state_;
490  }
491  }
492  else if (current_state_time_ > tm)
493  { // fixed display time per state
494  current_state_time_ = 0.0;
495  ++current_state_;
496  }
497 
498  if (current_state_ == previous_state)
499  return;
500 
501  if (current_state_ < waypoint_count)
502  {
506  for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
507  trajectory_trail_[i]->setVisible(
508  std::min(waypoint_count - 1, static_cast<int>(i) * trail_step_size_property_->getInt()) <= current_state_);
509  }
510  else
511  {
512  animating_path_ = false; // animation finished
513  if (trajectory_slider_panel_) // make sure we move the slider to the end
514  // so the user can re-play
516  display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(waypoint_count - 1));
520  }
521  }
522  // set visibility
526 }
527 
528 void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg)
529 {
530  // Error check
531  if (!robot_state_ || !robot_model_)
532  {
533  ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot state or robot model loaded");
534  return;
535  }
536 
537  if (!msg->model_id.empty() && msg->model_id != robot_model_->getName())
538  ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", msg->model_id.c_str(),
539  robot_model_->getName().c_str());
540 
541  robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, ""));
542  try
543  {
544  for (std::size_t i = 0; i < msg->trajectory.size(); ++i)
545  {
546  if (t->empty())
547  {
548  t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]);
549  }
550  else
551  {
553  tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->trajectory[i]);
554  t->append(tmp, 0.0);
555  }
556  }
557  display_->setStatus(rviz::StatusProperty::Ok, "Trajectory", "");
558  }
559  catch (const moveit::Exception& e)
560  {
561  display_->setStatus(rviz::StatusProperty::Error, "Trajectory", e.what());
562  return;
563  }
564 
565  if (!t->empty())
566  {
567  boost::mutex::scoped_lock lock(update_trajectory_message_);
571  }
572 }
573 
575 {
578 }
579 
581 {
584  else
585  unsetRobotColor(&(display_path_robot_->getRobot()));
586 }
587 
589 {
590  for (auto& link : robot->getLinks())
591  link.second->unsetColor();
592 }
593 
595 {
596  default_attached_object_color_.r = color.redF();
597  default_attached_object_color_.g = color.greenF();
598  default_attached_object_color_.b = color.blueF();
599  default_attached_object_color_.a = color.alphaF();
600 
602  {
603  display_path_robot_->setDefaultAttachedObjectColor(default_attached_object_color_);
604  display_path_robot_->updateAttachedObjectColors(default_attached_object_color_);
605  }
606  for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
607  r->updateAttachedObjectColors(default_attached_object_color_);
608 }
609 
610 void TrajectoryVisualization::setRobotColor(rviz::Robot* robot, const QColor& color)
611 {
612  for (auto& link : robot->getLinks())
613  robot->getLink(link.first)->setColor(color.redF(), color.greenF(), color.blueF());
614 }
615 
617 {
619  return;
620 
621  if (enable)
623  else
625 }
626 
628 {
629  robot_model_.reset();
630  robot_state_.reset();
631 }
632 
633 } // namespace moveit_rviz_plugin
moveit_rviz_plugin::TrajectoryVisualization::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: trajectory_visualization.h:153
rviz::BoolProperty::getBool
virtual bool getBool() const
rviz::Display::isEnabled
bool isEnabled() const
moveit_rviz_plugin::TrajectoryVisualization::TrajectoryVisualization
TrajectoryVisualization(rviz::Property *widget, rviz::Display *display)
Playback a trajectory from a planned path.
Definition: trajectory_visualization.cpp:93
rviz::RosTopicProperty
rviz::ColorProperty::getColor
virtual QColor getColor() const
moveit_rviz_plugin::TrajectoryPanel::onInitialize
void onInitialize() override
Definition: trajectory_panel.cpp:80
moveit_rviz_plugin::TrajectoryVisualization::context_
rviz::DisplayContext * context_
Definition: trajectory_visualization.h:159
moveit_rviz_plugin::TrajectoryVisualization::reset
virtual void reset()
Definition: trajectory_visualization.cpp:235
moveit_rviz_plugin::TrajectoryVisualization::getStateDisplayTime
float getStateDisplayTime()
get time to show each single robot state
Definition: trajectory_visualization.cpp:387
rviz::StatusProperty::Error
Error
moveit_rviz_plugin::TrajectoryVisualization::interruptCurrentDisplay
void interruptCurrentDisplay()
Definition: trajectory_visualization.cpp:378
moveit_rviz_plugin::TrajectoryVisualization::use_sim_time_property_
rviz::BoolProperty * use_sim_time_property_
Definition: trajectory_visualization.h:171
moveit_rviz_plugin::TrajectoryVisualization::animating_path_
bool animating_path_
Definition: trajectory_visualization.h:146
moveit_rviz_plugin::TrajectoryVisualization::trail_step_size_property_
rviz::IntProperty * trail_step_size_property_
Definition: trajectory_visualization.h:176
moveit_rviz_plugin::TrajectoryVisualization::state_display_time_property_
rviz::EditableEnumProperty * state_display_time_property_
Definition: trajectory_visualization.h:167
moveit_rviz_plugin::TrajectoryVisualization::trajectorySliderPanelVisibilityChange
void trajectorySliderPanelVisibilityChange(bool enable)
Definition: trajectory_visualization.cpp:648
moveit_rviz_plugin::TrajectoryVisualization::onEnable
void onEnable()
Definition: trajectory_visualization.cpp:350
rviz::EditableEnumProperty
rviz::BoolProperty
r
r
int_property.h
ros::Subscriber::getTopic
std::string getTopic() const
moveit_rviz_plugin::TrajectoryVisualization::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: trajectory_visualization.h:152
moveit_rviz_plugin::TrajectoryVisualization::changedShowTrail
void changedShowTrail()
Definition: trajectory_visualization.cpp:259
ros::Subscriber::shutdown
void shutdown()
moveit_rviz_plugin::TrajectoryPanel::onEnable
void onEnable()
Definition: trajectory_panel.cpp:112
rviz::Property::getIcon
virtual QIcon getIcon() const
moveit_rviz_plugin::TrajectoryVisualization::displaying_trajectory_message_
robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_
Definition: trajectory_visualization.h:142
moveit_rviz_plugin::TrajectoryVisualization::trajectory_trail_
std::vector< RobotStateVisualizationUniquePtr > trajectory_trail_
Definition: trajectory_visualization.h:144
rviz::Property::getValue
virtual QVariant getValue() const
moveit_rviz_plugin::TrajectoryVisualization::current_state_
int current_state_
Definition: trajectory_visualization.h:148
robot_trajectory::RobotTrajectory
moveit_rviz_plugin::TrajectoryVisualization::changedRobotColor
void changedRobotColor()
Definition: trajectory_visualization.cpp:606
moveit_rviz_plugin::TrajectoryVisualization::trajectory_slider_dock_panel_
rviz::PanelDockWidget * trajectory_slider_dock_panel_
Definition: trajectory_visualization.h:162
moveit_rviz_plugin::TrajectoryVisualization::onRobotModelLoaded
void onRobotModelLoaded(const moveit::core::RobotModelConstPtr &robot_model)
Definition: trajectory_visualization.cpp:212
float_property.h
rviz::ColorProperty
rviz::Display
rviz::FloatProperty
rviz::Property
moveit_rviz_plugin::TrajectoryVisualization::display_path_visual_enabled_property_
rviz::BoolProperty * display_path_visual_enabled_property_
Definition: trajectory_visualization.h:165
moveit_rviz_plugin::TrajectoryVisualization::onInitialize
void onInitialize(Ogre::SceneNode *scene_node, rviz::DisplayContext *context, const ros::NodeHandle &update_nh)
Definition: trajectory_visualization.cpp:179
moveit_rviz_plugin::TrajectoryPanel::onDisable
void onDisable()
Definition: trajectory_panel.cpp:118
rviz::Display::setStatus
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
moveit_rviz_plugin::TrajectoryVisualization::enable_robot_color_property_
rviz::BoolProperty * enable_robot_color_property_
Definition: trajectory_visualization.h:175
moveit_rviz_plugin::TrajectoryVisualization::drop_displaying_trajectory_
bool drop_displaying_trajectory_
Definition: trajectory_visualization.h:147
bool_property.h
editable_enum_property.h
moveit_rviz_plugin::TrajectoryVisualization::scene_node_
Ogre::SceneNode * scene_node_
Definition: trajectory_visualization.h:158
moveit_rviz_plugin::TrajectoryVisualization::update
virtual void update(float wall_dt, float sim_dt)
Definition: trajectory_visualization.cpp:439
moveit_rviz_plugin::TrajectoryVisualization::setDefaultAttachedObjectColor
void setDefaultAttachedObjectColor(const QColor &color)
Definition: trajectory_visualization.cpp:626
moveit_rviz_plugin::TrajectoryVisualization::clearRobotModel
void clearRobotModel()
Definition: trajectory_visualization.cpp:659
rviz::FloatProperty::getFloat
virtual float getFloat() const
moveit_rviz_plugin::TrajectoryVisualization::update_nh_
ros::NodeHandle update_nh_
Definition: trajectory_visualization.h:160
rviz::Property::setValue
virtual bool setValue(const QVariant &new_value)
window_manager_interface.h
moveit_rviz_plugin::TrajectoryVisualization::loop_display_property_
rviz::BoolProperty * loop_display_property_
Definition: trajectory_visualization.h:170
ROS_ERROR_STREAM_NAMED
#define ROS_ERROR_STREAM_NAMED(name, args)
rviz::StringProperty::getStdString
std::string getStdString()
rviz::StatusProperty::Ok
Ok
moveit_rviz_plugin::TrajectoryVisualization::trajectory_topic_sub_
ros::Subscriber trajectory_topic_sub_
Definition: trajectory_visualization.h:145
moveit_rviz_plugin::TrajectoryVisualization::trajectory_slider_panel_
TrajectoryPanel * trajectory_slider_panel_
Definition: trajectory_visualization.h:161
moveit_rviz_plugin::TrajectoryVisualization::default_attached_object_color_
std_msgs::ColorRGBA default_attached_object_color_
Definition: trajectory_visualization.h:136
moveit_rviz_plugin::TrajectoryVisualization::incomingDisplayTrajectory
void incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr &msg)
ROS callback for an incoming path message.
Definition: trajectory_visualization.cpp:560
value
float value
ROS_WARN
#define ROS_WARN(...)
moveit_rviz_plugin::TrajectoryVisualization::changedDisplayPathVisualEnabled
void changedDisplayPathVisualEnabled()
Slot Event Functions.
Definition: trajectory_visualization.cpp:324
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
rviz::WindowManagerInterface::getParentWindow
virtual QWidget * getParentWindow()=0
rviz::DisplayContext
rviz::Robot
moveit_rviz_plugin::TrajectoryVisualization::display_path_robot_
RobotStateVisualizationPtr display_path_robot_
Definition: trajectory_visualization.h:135
moveit_rviz_plugin::TrajectoryVisualization::changedStateDisplayTime
void changedStateDisplayTime()
Definition: trajectory_visualization.cpp:335
moveit_rviz_plugin::TrajectoryVisualization::current_state_time_
float current_state_time_
Definition: trajectory_visualization.h:149
moveit_rviz_plugin::TrajectoryVisualization::display_path_collision_enabled_property_
rviz::BoolProperty * display_path_collision_enabled_property_
Definition: trajectory_visualization.h:166
moveit_rviz_plugin::TrajectoryPanel::pauseButton
void pauseButton(bool check)
Definition: trajectory_panel.cpp:137
moveit_rviz_plugin::TrajectoryVisualization::onDisable
void onDisable()
Definition: trajectory_visualization.cpp:367
moveit_rviz_plugin
Definition: motion_planning_display.h:80
moveit_rviz_plugin::TrajectoryVisualization::changedLoopDisplay
void changedLoopDisplay()
Definition: trajectory_visualization.cpp:252
moveit_rviz_plugin::TrajectoryVisualization::interrupt_display_property_
rviz::BoolProperty * interrupt_display_property_
Definition: trajectory_visualization.h:173
moveit_rviz_plugin::TrajectoryVisualization::trail_display_property_
rviz::BoolProperty * trail_display_property_
Definition: trajectory_visualization.h:172
rviz::DisplayContext::getWindowManager
virtual WindowManagerInterface * getWindowManager() const=0
moveit_rviz_plugin::TrajectoryVisualization::clearTrajectoryTrail
void clearTrajectoryTrail()
Definition: trajectory_visualization.cpp:247
rviz::Property::getName
virtual QString getName() const
moveit_rviz_plugin::TrajectoryPanel::isPaused
bool isPaused() const
Definition: trajectory_panel.h:139
moveit_rviz_plugin::TrajectoryPanel::setSliderPosition
void setSliderPosition(int position)
Definition: trajectory_panel.cpp:153
moveit_rviz_plugin::TrajectoryVisualization::changedTrajectoryTopic
void changedTrajectoryTopic()
Definition: trajectory_visualization.cpp:313
moveit_rviz_plugin::TrajectoryVisualization::changedTrailStepSize
void changedTrailStepSize()
Definition: trajectory_visualization.cpp:300
rviz::WindowManagerInterface::addPane
virtual PanelDockWidget * addPane(const QString &name, QWidget *pane, Qt::DockWidgetArea area=Qt::LeftDockWidgetArea, bool floating=false)=0
moveit_rviz_plugin::TrajectoryVisualization::~TrajectoryVisualization
~TrajectoryVisualization() override
Definition: trajectory_visualization.cpp:168
property.h
moveit_rviz_plugin::TrajectoryVisualization::widget_
rviz::Property * widget_
Definition: trajectory_visualization.h:157
rviz::StringProperty::setStdString
bool setStdString(const std::string &std_str)
moveit_rviz_plugin::TrajectoryVisualization::changedDisplayPathCollisionEnabled
void changedDisplayPathCollisionEnabled()
Definition: trajectory_visualization.cpp:339
rviz::Robot::getLinks
const M_NameToLink & getLinks() const
moveit_rviz_plugin::TrajectoryVisualization::trajectory_message_to_display_
robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_
Definition: trajectory_visualization.h:143
moveit_rviz_plugin::TrajectoryVisualization::enabledRobotColor
void enabledRobotColor()
Definition: trajectory_visualization.cpp:612
rviz::Property::changed
void changed()
moveit_rviz_plugin::TrajectoryVisualization::trajectory_topic_property_
rviz::RosTopicProperty * trajectory_topic_property_
Definition: trajectory_visualization.h:168
rviz::IntProperty::getInt
virtual int getInt() const
moveit_rviz_plugin::TrajectoryVisualization::changedRobotPathAlpha
void changedRobotPathAlpha()
Definition: trajectory_visualization.cpp:306
moveit_rviz_plugin::TrajectoryVisualization::unsetRobotColor
void unsetRobotColor(rviz::Robot *robot)
Definition: trajectory_visualization.cpp:620
moveit_rviz_plugin::TrajectoryVisualization::dropTrajectory
void dropTrajectory()
Definition: trajectory_visualization.cpp:434
robot_state_visualization.h
rviz::Robot::getLink
RobotLink * getLink(const std::string &name)
rviz::WindowManagerInterface
moveit_rviz_plugin::TrajectoryVisualization::robot_path_alpha_property_
rviz::FloatProperty * robot_path_alpha_property_
Definition: trajectory_visualization.h:169
trajectory_tools.h
moveit::Exception
string_property.h
moveit_rviz_plugin::TrajectoryVisualization::display_
rviz::Display * display_
Definition: trajectory_visualization.h:156
moveit_rviz_plugin::TrajectoryVisualization::setRobotColor
void setRobotColor(rviz::Robot *robot, const QColor &color)
Definition: trajectory_visualization.cpp:642
rviz::PanelDockWidget::setIcon
void setIcon(const QIcon &icon)
t
dictionary t
rviz::PanelDockWidget::setWindowTitle
void setWindowTitle(const QString &title)
moveit_rviz_plugin::TrajectoryPanel::getSliderPosition
int getSliderPosition() const
Definition: trajectory_panel.h:134
trajectory_visualization.h
moveit_rviz_plugin::TrajectoryVisualization::robot_color_property_
rviz::ColorProperty * robot_color_property_
Definition: trajectory_visualization.h:174
color_property.h
ros_topic_property.h
ros::NodeHandle
robot.h
moveit_rviz_plugin::TrajectoryPanel::update
void update(int way_point_count)
Definition: trajectory_panel.cpp:124
rviz::IntProperty
display_context.h
moveit_rviz_plugin::TrajectoryVisualization::update_trajectory_message_
boost::mutex update_trajectory_message_
Definition: trajectory_visualization.h:150
moveit_rviz_plugin::TrajectoryVisualization::setName
void setName(const QString &name)
Definition: trajectory_visualization.cpp:206


visualization
Author(s): Ioan Sucan , Dave Coleman , Sachin Chitta
autogenerated on Sat Mar 15 2025 02:27:26