37 #include <boost/algorithm/string.hpp>
38 #include <boost/algorithm/string/replace.hpp>
62 : animating_path_(false)
63 , drop_displaying_trajectory_(false)
67 , trajectory_slider_panel_(nullptr)
68 , trajectory_slider_dock_panel_(nullptr)
70 trajectory_topic_property_ =
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);
76 display_path_visual_enabled_property_ =
78 "Indicates whether the geometry of the robot as defined for "
79 "visualisation purposes should be displayed",
80 widget, SLOT(changedDisplayPathVisualEnabled()),
this);
82 display_path_collision_enabled_property_ =
84 "Indicates whether the geometry of the robot as defined "
85 "for collision detection purposes should be displayed",
86 widget, SLOT(changedDisplayPathCollisionEnabled()),
this);
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);
93 state_display_time_property_ =
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");
106 "Indicates whether simulation time or wall-time is "
107 "used for state display timing.",
111 "Indicates whether the last received path "
112 "is to be animated in a loop",
113 widget, SLOT(changedLoopDisplay()),
this);
115 trail_display_property_ =
116 new rviz::BoolProperty(
"Show Trail",
false,
"Show a path trail", widget, SLOT(changedShowTrail()),
this);
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);
124 interrupt_display_property_ =
126 "Immediately show newly planned trajectory, interrupting the currently displayed one.",
130 "Robot Color", QColor(150, 50, 150),
"The color of the animated robot", widget, SLOT(changedRobotColor()),
this);
133 "Color Enabled",
false,
"Specifies whether robot coloring is enabled", widget, SLOT(enabledRobotColor()),
this);
241 trajectory_trail_.resize((
int)std::ceil((
t->getWayPointCount() + stepsize - 1) / (
float)stepsize));
244 int waypoint_i = std::min(i * stepsize,
t->getWayPointCount() - 1);
246 "Trail Robot " + boost::lexical_cast<std::string>(i),
nullptr);
256 for (
const auto& [link_name, robot_link] : robot_path_links)
339 r->setVisible(
false);
357 constexpr
char default_time_string[] =
"3x";
358 constexpr
float default_time_value = -3.0f;
365 if (tm.back() ==
'x')
369 else if (tm.back() ==
's')
376 return default_time_value;
379 tm.resize(tm.size() - 1);
380 boost::trim_right(tm);
385 value = boost::lexical_cast<float>(tm);
387 catch (
const boost::bad_lexical_cast& ex)
390 return default_time_value;
396 return default_time_value;
480 const float rt_factor = -tm;
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(),
544 for (std::size_t i = 0; i < msg->trajectory.size(); ++i)
548 t->setRobotTrajectoryMsg(*
robot_state_, msg->trajectory_start, msg->trajectory[i]);
553 tmp.setRobotTrajectoryMsg(
t->getLastWayPoint(), msg->trajectory[i]);
590 for (
auto& link : robot->
getLinks())
591 link.second->unsetColor();
613 robot->
getLink(link.first)->
setColor(color.redF(), color.greenF(), color.blueF());