37#include <boost/algorithm/string.hpp>
38#include <boost/algorithm/string/replace.hpp>
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>
60#include <rclcpp/qos.hpp>
65using namespace std::placeholders;
71 rviz_common::Display* display)
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);
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);
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);
100 "Robot Alpha", 0.5f,
"Specifies the alpha for the robot links", widget, SLOT(changedRobotPathAlpha()),
this);
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);
117 "Use Sim Time",
false,
"Indicates whether simulation time or wall-time is used for state display timing.", widget,
121 "Indicates whether the last received path "
122 "is to be animated in a loop",
123 widget, SLOT(changedLoopDisplay()),
this);
125 trail_display_property_ =
new rviz_common::properties::BoolProperty(
"Show Trail",
false,
"Show a path trail", widget,
126 SLOT(changedShowTrail()),
this);
129 "Specifies the step size of the samples "
130 "shown in the trajectory trail.",
131 widget, SLOT(changedTrailStepSize()),
this);
135 "Interrupt Display",
false,
136 "Immediately show newly planned trajectory, interrupting the currently displayed one.", widget);
139 "Robot Color", QColor(150, 50, 150),
"The color of the animated robot", widget, SLOT(changedRobotColor()),
this);
142 "Color Enabled",
false,
"Specifies whether robot coloring is enabled", widget, SLOT(enabledRobotColor()),
this);
157 rviz_common::DisplayContext* context)
164 auto ros_node_abstraction =
context_->getRosNodeAbstraction().lock();
165 if (!ros_node_abstraction)
167 RVIZ_COMMON_LOG_WARNING(
"Unable to lock weak_ptr from DisplayContext in TrajectoryVisualization constructor");
178 rviz_common::WindowManagerInterface* window_context =
context_->getWindowManager();
186 SLOT(trajectorySliderPanelVisibilityChange(
bool)));
208 RCLCPP_ERROR_STREAM(
logger_,
"No robot model found");
222 changedTrajectoryTopic();
242void TrajectoryVisualization::changedLoopDisplay()
249void TrajectoryVisualization::changedShowTrail()
264 static_cast<int>(std::ceil((t->getWayPointCount() + stepsize - 1) /
static_cast<float>(stepsize))));
267 int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1);
269 std::make_unique<RobotStateVisualization>(
scene_node_,
context_,
"Trail Robot " + std::to_string(i),
nullptr);
283void TrajectoryVisualization::changedTrailStepSize()
289void TrajectoryVisualization::changedRobotPathAlpha()
296void TrajectoryVisualization::changedTrajectoryTopic()
303 [
this](
const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) {
309void TrajectoryVisualization::changedDisplayPathVisualEnabled()
320void TrajectoryVisualization::changedStateDisplayTime()
324void TrajectoryVisualization::changedDisplayPathCollisionEnabled()
337 changedRobotPathAlpha();
349 changedTrajectoryTopic();
356 r->setVisible(
false);
374 constexpr char default_time_string[] =
"3x";
375 constexpr double default_time_value = -3.0;
382 if (tm.back() ==
'x')
386 else if (tm.back() ==
's')
393 return default_time_value;
396 tm.resize(tm.size() - 1);
397 boost::trim_right(tm);
402 value = std::stof(tm);
404 catch (
const std::invalid_argument& ex)
407 return default_time_value;
413 return default_time_value;
480 current_state_time_ += std::chrono::duration_cast<std::chrono::duration<double>>(sim_dt).count();
484 current_state_time_ += std::chrono::duration_cast<std::chrono::duration<double>>(wall_dt).count();
501 const double rt_factor = -tm;
556 update(std::chrono::nanoseconds(std::lround(wall_dt)), std::chrono::nanoseconds(std::lround(sim_dt)));
564 RCLCPP_ERROR_STREAM(
logger_,
"No robot state or robot model loaded");
568 if (!msg->model_id.empty() && msg->model_id !=
robot_model_->getName())
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());
576 auto t = std::make_shared<robot_trajectory::RobotTrajectory>(
robot_model_,
"");
579 for (std::size_t i = 0; i < msg->trajectory.size(); ++i)
583 t->setRobotTrajectoryMsg(*
robot_state_, msg->trajectory_start, msg->trajectory[i]);
592 display_->setStatus(rviz_common::properties::StatusProperty::Ok,
"Trajectory",
"");
596 display_->setStatus(rviz_common::properties::StatusProperty::Error,
"Trajectory", e.what());
609void TrajectoryVisualization::changedRobotColor()
615void TrajectoryVisualization::enabledRobotColor()
629 for (
auto& link : robot->getLinks())
630 link.second->unsetColor();
651 for (
auto& link : robot->getLinks())
652 robot->getLink(link.first)->setColor(color.redF(), color.greenF(), color.blueF());
655void TrajectoryVisualization::trajectorySliderPanelVisibilityChange(
bool enable)
This may be thrown if unrecoverable errors occur.
void pauseButton(bool check)
rviz_common::properties::BoolProperty * enable_robot_color_property_
void clearTrajectoryTrail()
void incomingDisplayTrajectory(const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr &msg)
ROS callback for an incoming path message.
Ogre::SceneNode * scene_node_
rviz_common::DisplayContext * context_
rviz_common::properties::BoolProperty * display_path_visual_enabled_property_
void setName(const QString &name)
rviz_common::properties::BoolProperty * interrupt_display_property_
robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_
robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_
std::mutex update_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)
TrajectoryPanel * trajectory_slider_panel_
rviz_common::properties::BoolProperty * display_path_collision_enabled_property_
moveit::core::RobotStatePtr robot_state_
rviz_common::properties::RosTopicProperty * trajectory_topic_property_
moveit::core::RobotModelConstPtr robot_model_
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.
std_msgs::msg::ColorRGBA default_attached_object_color_
rviz_common::properties::Property * widget_
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)
double current_state_time_
rclcpp::Node::SharedPtr node_
rviz_common::properties::EditableEnumProperty * state_display_time_property_
void setDefaultAttachedObjectColor(const QColor &color)
rviz_common::properties::IntProperty * trail_step_size_property_
void interruptCurrentDisplay()
rviz_common::Display * display_
std::vector< RobotStateVisualizationUniquePtr > trajectory_trail_
void unsetRobotColor(rviz_default_plugins::robot::Robot *robot)
~TrajectoryVisualization() override
void onRobotModelLoaded(const moveit::core::RobotModelConstPtr &robot_model)
RobotStateVisualizationPtr display_path_robot_
rviz_common::properties::FloatProperty * robot_path_alpha_property_
bool drop_displaying_trajectory_
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.
rclcpp::Logger getLogger()