moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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_default_plugins/robot/robot.hpp>
47#include <rviz_common/display_context.hpp>
48#include <rviz_common/properties/bool_property.hpp>
49#include <rviz_common/properties/color_property.hpp>
50#include <rviz_common/properties/editable_enum_property.hpp>
51#include <rviz_common/properties/float_property.hpp>
52#include <rviz_common/properties/int_property.hpp>
53#include <rviz_common/properties/property.hpp>
54#include <rviz_common/properties/ros_topic_property.hpp>
55#include <rviz_common/properties/string_property.hpp>
56#include <rviz_default_plugins/robot/robot_link.hpp>
57#include <rviz_common/window_manager_interface.hpp>
58#include <rviz_common/logging.hpp>
59
60#include <rclcpp/qos.hpp>
61
62#include <cmath>
63#include <string>
64
65using namespace std::placeholders;
66
67namespace moveit_rviz_plugin
68{
69
70TrajectoryVisualization::TrajectoryVisualization(rviz_common::properties::Property* widget,
71 rviz_common::Display* display)
72 : animating_path_(false)
74 , current_state_(-1)
75 , display_(display)
76 , widget_(widget)
79 , logger_(moveit::getLogger("moveit.ros.trajectory_visualization"))
80{
81 trajectory_topic_property_ = new rviz_common::properties::RosTopicProperty(
82 "Trajectory Topic", "/display_planned_path",
83 rosidl_generator_traits::data_type<moveit_msgs::msg::DisplayTrajectory>(),
84 "The topic on which the moveit_msgs::msg::DisplayTrajectory messages are received", widget,
85 SLOT(changedTrajectoryTopic()), this);
86
88 new rviz_common::properties::BoolProperty("Show Robot Visual", true,
89 "Indicates whether the geometry of the robot as defined for "
90 "visualisation purposes should be displayed",
91 widget, SLOT(changedDisplayPathVisualEnabled()), this);
92
94 new rviz_common::properties::BoolProperty("Show Robot Collision", false,
95 "Indicates whether the geometry of the robot as defined "
96 "for collision detection purposes should be displayed",
97 widget, SLOT(changedDisplayPathCollisionEnabled()), this);
98
99 robot_path_alpha_property_ = new rviz_common::properties::FloatProperty(
100 "Robot Alpha", 0.5f, "Specifies the alpha for the robot links", widget, SLOT(changedRobotPathAlpha()), this);
101 robot_path_alpha_property_->setMin(0.0);
102 robot_path_alpha_property_->setMax(1.0);
103
105 new rviz_common::properties::EditableEnumProperty("State Display Time", "3x",
106 "Replay speed of trajectory. Either as factor of its time"
107 "parameterization or as constant time (s) per waypoint.",
108 widget, SLOT(changedStateDisplayTime()), this);
109 state_display_time_property_->addOptionStd("3x");
110 state_display_time_property_->addOptionStd("1x");
111 state_display_time_property_->addOptionStd("0.5x");
112 state_display_time_property_->addOptionStd("0.05s");
113 state_display_time_property_->addOptionStd("0.1s");
114 state_display_time_property_->addOptionStd("0.5s");
115
116 use_sim_time_property_ = new rviz_common::properties::BoolProperty(
117 "Use Sim Time", false, "Indicates whether simulation time or wall-time is used for state display timing.", widget,
118 nullptr, this);
119
120 loop_display_property_ = new rviz_common::properties::BoolProperty("Loop Animation", false,
121 "Indicates whether the last received path "
122 "is to be animated in a loop",
123 widget, SLOT(changedLoopDisplay()), this);
124
125 trail_display_property_ = new rviz_common::properties::BoolProperty("Show Trail", false, "Show a path trail", widget,
126 SLOT(changedShowTrail()), this);
127
128 trail_step_size_property_ = new rviz_common::properties::IntProperty("Trail Step Size", 1,
129 "Specifies the step size of the samples "
130 "shown in the trajectory trail.",
131 widget, SLOT(changedTrailStepSize()), this);
132 trail_step_size_property_->setMin(1);
133
134 interrupt_display_property_ = new rviz_common::properties::BoolProperty(
135 "Interrupt Display", false,
136 "Immediately show newly planned trajectory, interrupting the currently displayed one.", widget);
137
138 robot_color_property_ = new rviz_common::properties::ColorProperty(
139 "Robot Color", QColor(150, 50, 150), "The color of the animated robot", widget, SLOT(changedRobotColor()), this);
140
141 enable_robot_color_property_ = new rviz_common::properties::BoolProperty(
142 "Color Enabled", false, "Specifies whether robot coloring is enabled", widget, SLOT(enabledRobotColor()), this);
143}
144
155
156void TrajectoryVisualization::onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node,
157 rviz_common::DisplayContext* context)
158{
159 // Save pointers for later use
160 scene_node_ = scene_node;
161 context_ = context;
162 node_ = node;
163
164 auto ros_node_abstraction = context_->getRosNodeAbstraction().lock();
165 if (!ros_node_abstraction)
166 {
167 RVIZ_COMMON_LOG_WARNING("Unable to lock weak_ptr from DisplayContext in TrajectoryVisualization constructor");
168 return;
169 }
170 trajectory_topic_property_->initialize(ros_node_abstraction);
171
172 // Load trajectory robot
173 display_path_robot_ = std::make_shared<RobotStateVisualization>(scene_node_, context_, "Planned Path", widget_);
175 display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
176 display_path_robot_->setVisible(false);
177
178 rviz_common::WindowManagerInterface* window_context = context_->getWindowManager();
179 if (window_context)
180 {
181 trajectory_slider_panel_ = new TrajectoryPanel(window_context->getParentWindow());
183 window_context->addPane(display_->getName() + " - Trajectory Slider", trajectory_slider_panel_);
184 trajectory_slider_dock_panel_->setIcon(display_->getIcon());
185 connect(trajectory_slider_dock_panel_, SIGNAL(visibilityChanged(bool)), this,
186 SLOT(trajectorySliderPanelVisibilityChange(bool)));
187 trajectory_slider_panel_->onInitialize();
188 }
189
190 trajectory_topic_sub_ = node_->create_subscription<moveit_msgs::msg::DisplayTrajectory>(
191 trajectory_topic_property_->getStdString(), rclcpp::ServicesQoS(),
192 [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { return incomingDisplayTrajectory(msg); });
193}
194
195void TrajectoryVisualization::setName(const QString& name)
196{
198 trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider");
199}
200
201void TrajectoryVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model)
202{
203 robot_model_ = robot_model;
204
205 // Error check
206 if (!robot_model_)
207 {
208 RCLCPP_ERROR_STREAM(logger_, "No robot model found");
209 return;
210 }
211
212 // Load robot state
213 robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
214 robot_state_->setToDefaultValues();
215
216 // Load rviz robot
217 display_path_robot_->load(*robot_model_->getURDF());
218 enabledRobotColor(); // force-refresh to account for saved display configuration
219 // perform post-poned subscription to trajectory topic
220 // Check if topic name is empty
221 if (trajectory_topic_sub_->get_topic_name())
222 changedTrajectoryTopic();
223}
224
236
241
242void TrajectoryVisualization::changedLoopDisplay()
243{
247}
248
249void TrajectoryVisualization::changedShowTrail()
250{
252
253 if (!trail_display_property_->getBool())
254 return;
255 robot_trajectory::RobotTrajectoryPtr t = trajectory_message_to_display_;
256 if (!t)
258 if (!t)
259 return;
260
261 int stepsize = trail_step_size_property_->getInt();
262 // always include last trajectory point
263 trajectory_trail_.resize(
264 static_cast<int>(std::ceil((t->getWayPointCount() + stepsize - 1) / static_cast<float>(stepsize))));
265 for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
266 {
267 int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point
268 auto r =
269 std::make_unique<RobotStateVisualization>(scene_node_, context_, "Trail Robot " + std::to_string(i), nullptr);
270 r->load(*robot_model_->getURDF());
271 r->setVisualVisible(display_path_visual_enabled_property_->getBool());
272 r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
273 r->setAlpha(robot_path_alpha_property_->getFloat());
274 r->update(t->getWayPointPtr(waypoint_i), default_attached_object_color_);
275 if (enable_robot_color_property_->getBool())
276 setRobotColor(&(r->getRobot()), robot_color_property_->getColor());
277 r->setVisible(display_->isEnabled() && (!animating_path_ || waypoint_i <= current_state_));
278 r->updateAttachedObjectColors(default_attached_object_color_);
279 trajectory_trail_[i] = std::move(r);
280 }
281}
282
283void TrajectoryVisualization::changedTrailStepSize()
284{
285 if (trail_display_property_->getBool())
286 changedShowTrail();
287}
288
289void TrajectoryVisualization::changedRobotPathAlpha()
290{
291 display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat());
292 for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
293 r->setAlpha(robot_path_alpha_property_->getFloat());
294}
295
296void TrajectoryVisualization::changedTrajectoryTopic()
297{
298 // post-pone subscription if robot_state_ is not yet defined, i.e. onRobotModelLoaded() not yet called
299 if (!trajectory_topic_property_->getStdString().empty() && robot_state_)
300 {
301 trajectory_topic_sub_ = node_->create_subscription<moveit_msgs::msg::DisplayTrajectory>(
302 trajectory_topic_property_->getStdString(), rclcpp::ServicesQoS(),
303 [this](const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) {
304 return incomingDisplayTrajectory(msg);
305 });
306 }
307}
308
309void TrajectoryVisualization::changedDisplayPathVisualEnabled()
310{
311 if (display_->isEnabled())
312 {
315 for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
316 r->setVisualVisible(display_path_visual_enabled_property_->getBool());
317 }
318}
319
320void TrajectoryVisualization::changedStateDisplayTime()
321{
322}
323
324void TrajectoryVisualization::changedDisplayPathCollisionEnabled()
325{
326 if (display_->isEnabled())
327 {
328 display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
330 for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
331 r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
332 }
333}
334
336{
337 changedRobotPathAlpha(); // set alpha property
338
340 display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
342 for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
343 {
344 r->setVisualVisible(display_path_visual_enabled_property_->getBool());
345 r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
346 r->setVisible(true);
347 }
348
349 changedTrajectoryTopic(); // load topic at startup if default used
350}
351
353{
354 display_path_robot_->setVisible(false);
355 for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
356 r->setVisible(false);
358 animating_path_ = false;
360 trajectory_slider_panel_->onDisable();
361}
362
364{
365 // update() starts a new trajectory as soon as it is available
366 // interrupting may cause the newly received trajectory to interrupt
367 // hence, only interrupt when current_state_ already advanced past first
368 if (current_state_ > 0)
369 animating_path_ = false;
370}
371
373{
374 constexpr char default_time_string[] = "3x";
375 constexpr double default_time_value = -3.0;
376
377 std::string tm = state_display_time_property_->getStdString();
378 boost::trim(tm);
379
380 double type;
381
382 if (tm.back() == 'x')
383 {
384 type = -1.0f;
385 }
386 else if (tm.back() == 's')
387 {
388 type = 1.0f;
389 }
390 else
391 {
392 state_display_time_property_->setStdString(default_time_string);
393 return default_time_value;
394 }
395
396 tm.resize(tm.size() - 1);
397 boost::trim_right(tm);
398
399 double value;
400 try
401 {
402 value = std::stof(tm);
403 }
404 catch (const std::invalid_argument& ex)
405 {
406 state_display_time_property_->setStdString(default_time_string);
407 return default_time_value;
408 }
409
410 if (value <= 0)
411 {
412 state_display_time_property_->setStdString(default_time_string);
413 return default_time_value;
414 }
415
416 return type * value;
417}
418
423
424void TrajectoryVisualization::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds sim_dt)
425{
427 {
428 animating_path_ = false;
430 trajectory_slider_panel_->update(0);
432 }
433 if (!animating_path_)
434 { // finished last animation?
435 std::scoped_lock lock(update_trajectory_message_);
436
437 // new trajectory available to display?
439 {
440 animating_path_ = true;
442 changedShowTrail();
444 trajectory_slider_panel_->update(trajectory_message_to_display_->getWayPointCount());
445 }
447 {
448 if (loop_display_property_->getBool())
449 { // do loop? -> start over too
450 animating_path_ = true;
451 }
452 else if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible())
453 {
454 if (static_cast<unsigned int>(trajectory_slider_panel_->getSliderPosition()) >=
455 displaying_trajectory_message_->getWayPointCount() - 1)
456 {
457 return; // nothing more to do
458 }
459 else
460 {
461 animating_path_ = true;
462 }
463 }
464 }
466
467 if (animating_path_)
468 {
469 // restart animation
470 current_state_ = -1;
471 }
472 }
473
474 if (animating_path_)
475 {
476 int previous_state = current_state_;
477 int waypoint_count = displaying_trajectory_message_->getWayPointCount();
478 if (use_sim_time_property_->getBool())
479 {
480 current_state_time_ += std::chrono::duration_cast<std::chrono::duration<double>>(sim_dt).count();
481 }
482 else
483 {
484 current_state_time_ += std::chrono::duration_cast<std::chrono::duration<double>>(wall_dt).count();
485 }
486 double tm = getStateDisplayTime();
487
489 {
490 current_state_ = trajectory_slider_panel_->getSliderPosition();
491 current_state_time_ = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_);
492 }
493 else if (current_state_ < 0)
494 { // special case indicating restart of animation
495 current_state_ = 0;
497 }
498 else if (tm < 0.0)
499 {
500 // using realtime factors: skip to next waypoint based on elapsed display time
501 const double rt_factor = -tm; // negative tm is the intended realtime factor
502 while (current_state_ < waypoint_count &&
503 (tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1) / rt_factor) <
505 {
507 // if we are stuck in the while loop we should move the robot along the path to keep up
508 if (tm < current_state_time_)
511 }
512 }
513 else if (current_state_time_ > tm)
514 { // fixed display time per state
517 }
518
519 if (current_state_ == previous_state)
520 return;
521
522 if (current_state_ < waypoint_count)
523 {
525 trajectory_slider_panel_->setSliderPosition(current_state_);
527 for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
528 {
529 trajectory_trail_[i]->setVisible(
530 std::min(waypoint_count - 1, static_cast<int>(i) * trail_step_size_property_->getInt()) <= current_state_);
531 }
532 }
533 else
534 {
535 animating_path_ = false; // animation finished
537 { // make sure we move the slider to the end
538 // so the user can re-play
539 trajectory_slider_panel_->setSliderPosition(waypoint_count);
540 }
541 display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(waypoint_count - 1));
542 display_path_robot_->setVisible(loop_display_property_->getBool());
544 trajectory_slider_panel_->pauseButton(true);
545 }
546 }
547 // set visibility
548 display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ &&
551 display_path_robot_->updateAttachedObjectColors(default_attached_object_color_);
552}
553
554void TrajectoryVisualization::update(double wall_dt, double sim_dt)
555{
556 update(std::chrono::nanoseconds(std::lround(wall_dt)), std::chrono::nanoseconds(std::lround(sim_dt)));
557}
558
559void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg)
560{
561 // Error check
562 if (!robot_state_ || !robot_model_)
563 {
564 RCLCPP_ERROR_STREAM(logger_, "No robot state or robot model loaded");
565 return;
566 }
567
568 if (!msg->model_id.empty() && msg->model_id != robot_model_->getName())
569 {
570 RCLCPP_WARN(logger_, "Received a trajectory to display for model '%s' but model '%s' was expected",
571 msg->model_id.c_str(), robot_model_->getName().c_str());
572 }
573
575
576 auto t = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, "");
577 try
578 {
579 for (std::size_t i = 0; i < msg->trajectory.size(); ++i)
580 {
581 if (t->empty())
582 {
583 t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]);
584 }
585 else
586 {
588 tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->trajectory[i]);
589 t->append(tmp, 0.0);
590 }
591 }
592 display_->setStatus(rviz_common::properties::StatusProperty::Ok, "Trajectory", "");
593 }
594 catch (const moveit::Exception& e)
595 {
596 display_->setStatus(rviz_common::properties::StatusProperty::Error, "Trajectory", e.what());
597 return;
598 }
599
600 if (!t->empty())
601 {
602 std::scoped_lock lock(update_trajectory_message_);
604 if (interrupt_display_property_->getBool())
606 }
607}
608
609void TrajectoryVisualization::changedRobotColor()
610{
611 if (enable_robot_color_property_->getBool())
612 setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor());
613}
614
615void TrajectoryVisualization::enabledRobotColor()
616{
617 if (enable_robot_color_property_->getBool())
618 {
619 setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor());
620 }
621 else
622 {
623 unsetRobotColor(&(display_path_robot_->getRobot()));
624 }
625}
626
627void TrajectoryVisualization::unsetRobotColor(rviz_default_plugins::robot::Robot* robot)
628{
629 for (auto& link : robot->getLinks())
630 link.second->unsetColor();
631}
632
634{
635 default_attached_object_color_.r = color.redF();
636 default_attached_object_color_.g = color.greenF();
637 default_attached_object_color_.b = color.blueF();
638 default_attached_object_color_.a = color.alphaF();
639
641 {
642 display_path_robot_->setDefaultAttachedObjectColor(default_attached_object_color_);
643 display_path_robot_->updateAttachedObjectColors(default_attached_object_color_);
644 }
645 for (const RobotStateVisualizationUniquePtr& r : trajectory_trail_)
646 r->updateAttachedObjectColors(default_attached_object_color_);
647}
648
649void TrajectoryVisualization::setRobotColor(rviz_default_plugins::robot::Robot* robot, const QColor& color)
650{
651 for (auto& link : robot->getLinks())
652 robot->getLink(link.first)->setColor(color.redF(), color.greenF(), color.blueF());
653}
654
655void TrajectoryVisualization::trajectorySliderPanelVisibilityChange(bool enable)
656{
658 return;
659
660 if (enable)
661 {
663 }
664 else
665 {
667 }
668}
669
671{
672 robot_model_.reset();
673 robot_state_.reset();
674}
675
676} // namespace moveit_rviz_plugin
This may be thrown if unrecoverable errors occur.
rviz_common::properties::BoolProperty * enable_robot_color_property_
void incomingDisplayTrajectory(const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr &msg)
ROS callback for an incoming path message.
rviz_common::properties::BoolProperty * display_path_visual_enabled_property_
rviz_common::properties::BoolProperty * interrupt_display_property_
robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_
robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_
rviz_common::PanelDockWidget * trajectory_slider_dock_panel_
void onInitialize(const rclcpp::Node::SharedPtr &node, Ogre::SceneNode *scene_node, rviz_common::DisplayContext *context)
rviz_common::properties::BoolProperty * trail_display_property_
virtual void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds sim_dt)
rviz_common::properties::BoolProperty * display_path_collision_enabled_property_
rviz_common::properties::RosTopicProperty * trajectory_topic_property_
rviz_common::properties::BoolProperty * use_sim_time_property_
TrajectoryVisualization(rviz_common::properties::Property *widget, rviz_common::Display *display)
Playback a trajectory from a planned path.
rviz_common::properties::BoolProperty * loop_display_property_
rviz_common::properties::ColorProperty * robot_color_property_
void setRobotColor(rviz_default_plugins::robot::Robot *robot, const QColor &color)
rviz_common::properties::EditableEnumProperty * state_display_time_property_
rviz_common::properties::IntProperty * trail_step_size_property_
std::vector< RobotStateVisualizationUniquePtr > trajectory_trail_
void unsetRobotColor(rviz_default_plugins::robot::Robot *robot)
void onRobotModelLoaded(const moveit::core::RobotModelConstPtr &robot_model)
rviz_common::properties::FloatProperty * robot_path_alpha_property_
rclcpp::Subscription< moveit_msgs::msg::DisplayTrajectory >::SharedPtr trajectory_topic_sub_
double getStateDisplayTime()
get time to show each single robot state
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & setRobotTrajectoryMsg(const moveit::core::RobotState &reference_state, const trajectory_msgs::msg::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
RobotTrajectory & append(const RobotTrajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
Add a specified part of a trajectory to the end of the current trajectory. The default (when start_in...
Main namespace for MoveIt.