diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index eb627c54..67fa1f18 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -88,8 +88,6 @@ TaskDisplay::TaskDisplay() : Display() connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)), task_list_model_.get(), SLOT(highlightStage(size_t))); - marker_visual_ = new MarkerVisualizationProperty("Markers", this); - tasks_property_ = new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this); } @@ -103,7 +101,6 @@ void TaskDisplay::onInitialize() { Display::onInitialize(); trajectory_visual_->onInitialize(scene_node_, context_); - marker_visual_->onInitialize(scene_node_, context_); #if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) // displays are loaded before panels, hence wait a little bit for the panel to be loaded @@ -246,16 +243,6 @@ void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionCon }); } -void TaskDisplay::clearMarkers() { - marker_visual_->clearMarkers(); -} - -void TaskDisplay::addMarkers(const DisplaySolutionPtr &s) { - if (!s) return; - for (size_t i=0, end = s->numSubSolutions(); i != end; ++i) { - marker_visual_->addMarkers(s->markersOfSubTrajectory(i)); - } -} void TaskDisplay::changedTaskSolutionTopic() { diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index 53e0c3a8..fd4e70fc 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -39,6 +39,7 @@ #pragma once #include +#include #ifndef Q_MOC_RUN #include "job_queue.h" @@ -63,8 +64,6 @@ namespace moveit_rviz_plugin { MOVEIT_CLASS_FORWARD(DisplaySolution) -class TaskSolutionVisualization; -class MarkerVisualizationProperty; class TaskListModel; class TaskDisplay : public rviz::Display @@ -87,8 +86,9 @@ public: TaskListModel& getTaskListModel() { return *task_list_model_; } TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); } - void clearMarkers(); - void addMarkers(const DisplaySolutionPtr &s); + + inline void clearMarkers() { trajectory_visual_->clearMarkers(); } + inline void addMarkers(const DisplaySolutionPtr &s) { trajectory_visual_->addMarkers(s); } protected: void onInitialize() override; @@ -132,7 +132,6 @@ protected: // Properties rviz::StringProperty* robot_description_property_; rviz::RosTopicProperty* task_solution_topic_property_; - MarkerVisualizationProperty* marker_visual_; rviz::Property* tasks_property_; }; diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h index 3cccf5db..e73a819f 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h @@ -21,6 +21,9 @@ class MarkerBase; namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) } +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(RobotState) +} } namespace moveit_rviz_plugin { @@ -36,20 +39,48 @@ MOVEIT_CLASS_FORWARD(MarkerVisualization) class MarkerVisualization { // list of all markers, attached to scene nodes in namespaces_ - typedef std::pair> MarkerData; + struct MarkerData { + visualization_msgs::MarkerPtr msg_; + std::shared_ptr marker_; + + MarkerData(const visualization_msgs::Marker& marker); + }; + struct NamespaceData { + Ogre::SceneNode* ns_node_ = nullptr; + // markers grouped by frame + std::map frames_; + }; + + // list of all markers std::deque markers_; // markers grouped by their namespace - std::map namespaces_; + std::map namespaces_; + + // planning_frame_ of scene + std::string planning_frame_; + // flag indicating that markers were created + bool markers_created_ = false; public: MarkerVisualization(const std::vector& markers, const planning_scene::PlanningScene& end_scene); ~MarkerVisualization(); - void createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node); - const std::map& namespaces() const { return namespaces_; } + /// did we successfully created all markers (and scene nodes)? + bool created() const { return markers_created_; } + /// create markers (placed at planning frame of scene) + bool createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node); + /// update marker position/orientation based on frames of given scene + robot_state + void update(const planning_scene::PlanningScene &end_scene, + const moveit::core::RobotState &robot_state); + const std::map& namespaces() const { return namespaces_; } void setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible); + +private: + void update(MarkerData &data, + const planning_scene::PlanningScene &end_scene, + const moveit::core::RobotState &robot_state) const; }; @@ -67,6 +98,7 @@ class MarkerVisualizationProperty: public rviz::BoolProperty Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to std::map namespaces_; // rviz properties for encountered namespaces std::list hosted_markers_; // list of hosted MarkerVisualization instances + rviz::BoolProperty* all_markers_at_once_; public: MarkerVisualizationProperty(const QString& name, Property* parent = nullptr); @@ -76,12 +108,21 @@ public: /// remove all hosted markers from display void clearMarkers(); - /// add all markers in MarkerVisualization for display + /// add markers in MarkerVisualization for display void addMarkers(MarkerVisualizationPtr markers); + /// update pose of all markers + void update(const planning_scene::PlanningScene &scene, + const moveit::core::RobotState &robot_state); + + bool allAtOnce() const; public Q_SLOTS: void onEnableChanged(); void onNSEnableChanged(); + void onAllAtOnceChanged(); + +Q_SIGNALS: + void allAtOnceChanged(bool); }; } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index f4f70cd1..b5f5d32c 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -80,6 +80,7 @@ MOVEIT_CLASS_FORWARD(PlanningSceneRender) MOVEIT_CLASS_FORWARD(DisplaySolution) class TaskSolutionPanel; +class MarkerVisualizationProperty; class TaskSolutionVisualization : public QObject { Q_OBJECT @@ -105,13 +106,18 @@ public: planning_scene::PlanningSceneConstPtr getScene() const { return scene_; } void showTrajectory(const moveit_task_constructor_msgs::Solution& msg); - void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s, bool lock); + void showTrajectory(const moveit_rviz_plugin::DisplaySolutionPtr& s, bool lock); void unlock(); + void clearMarkers(); + void addMarkers(const moveit_rviz_plugin::DisplaySolutionPtr &s); + public Q_SLOTS: void interruptCurrentDisplay(); private Q_SLOTS: + void onAllAtOnceChanged(bool); + // trajectory property slots void changedRobotVisualEnabled(); void changedRobotCollisionEnabled(); @@ -142,6 +148,8 @@ protected: PlanningSceneRenderPtr scene_render_; // render the robot RobotStateVisualizationPtr robot_render_; + // render markers + MarkerVisualizationProperty* marker_visual_; // Handle colouring of robot void setRobotColor(rviz::Robot* robot, const QColor& color); diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index fd26c848..2bb90777 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -13,6 +13,7 @@ #include #include +#include #include #include #include @@ -56,51 +57,38 @@ rviz::MarkerBase* createMarker(int marker_type, rviz::DisplayContext* context, O } } -namespace { -// express marker pose relative to planning frame (of end scene) -bool toPlanningFrame(visualization_msgs::Marker &marker, - const planning_scene::PlanningScene &scene) +// create MarkerData with nil marker_ pointer, just with a copy of message +MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::Marker& marker) { - if (marker.header.frame_id == scene.getPlanningFrame()) - return true; - - if (!scene.knowsFrameTransform(marker.header.frame_id)) { - ROS_WARN_ONCE("unknown frame '%s' for solution marker in namespace '%s'", - marker.header.frame_id.c_str(), marker.ns.c_str()); - return false; - } - - Eigen::Affine3d pose; - tf::poseMsgToEigen(marker.pose, pose); - const Eigen::Affine3d tm = scene.getFrameTransform(marker.header.frame_id); - tf::poseEigenToMsg(tm * pose, marker.pose); - marker.header.frame_id = scene.getPlanningFrame(); - return true; -} + msg_.reset(new visualization_msgs::Marker(marker)); + msg_->header.stamp = ros::Time(); + msg_->frame_locked = false; } MarkerVisualization::MarkerVisualization(const std::vector &markers, const planning_scene::PlanningScene &end_scene) { + planning_frame_ = end_scene.getPlanningFrame(); // remember marker message, postpone rviz::MarkerBase creation until later for (const auto& marker : markers) { - // create MarkerData with nil Marker pointer - MarkerData data; - data.first.reset(new visualization_msgs::Marker(marker)); - // express marker pose relative to planning frame of end_scene - if (!toPlanningFrame(const_cast(*data.first), end_scene)) - continue; - markers_.push_back(std::move(data)); + if (!end_scene.knowsFrameTransform(marker.header.frame_id)) { + ROS_WARN_ONCE("unknown frame '%s' for solution marker in namespace '%s'", + marker.header.frame_id.c_str(), marker.ns.c_str()); + continue; // ignore markers with unknown frame + } + + // remember marker message + markers_.emplace_back(marker); // remember namespace name - namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr)); + namespaces_.insert(std::make_pair(marker.ns, NamespaceData())); } } MarkerVisualization::~MarkerVisualization() { for (const auto& pair : namespaces_) { - if (pair.second) - pair.second->getCreator()->destroySceneNode(pair.second); + if (pair.second.ns_node_) + pair.second.ns_node_->getCreator()->destroySceneNode(pair.second.ns_node_); } } @@ -114,56 +102,124 @@ void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible) void MarkerVisualization::setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible) { - auto it = namespaces_.find(ns); + auto it = namespaces_.find(ns.toStdString()); if (it == namespaces_.end()) return; - setVisibility(it->second, parent_scene_node, visible); + setVisibility(it->second.ns_node_, parent_scene_node, visible); } -void MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::SceneNode *parent_scene_node) +bool MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::SceneNode *parent_scene_node) { - std::string planning_frame; + if (markers_created_) + return true; // already called before + + // fetch transform from planning_frame_ to rviz' fixed frame + const std::string& fixed_frame = context->getFrameManager()->getFixedFrame(); Ogre::Quaternion quat; Ogre::Vector3 pos; - for (MarkerData& data : markers_) { - if (data.second) continue; - - auto ns_it = namespaces_.find(QString::fromStdString(data.first->ns)); - Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before! - if (ns_it->second == nullptr) // create scene node for this namespace - ns_it->second = parent_scene_node->getCreator()->createSceneNode(); - - data.second.reset(createMarker(data.first->type, context, ns_it->second)); - if (data.second) { - // rviz::MarkerBase::setMessage() initializes the marker - data.second->setMessage(data.first); - // ... and sets its position + orientation relative to rviz' fixed frame - // however, we want the marker to be placed w.r.t. planning frame = msg.header.frame_id - Q_ASSERT(!data.first->header.frame_id.empty()); - if (planning_frame.empty()) { // determine transform once - planning_frame = data.first->header.frame_id; - // transform from fixed frame to planning_frame - tf::TransformListener* tf = context->getFrameManager()->getTFClient(); - tf::StampedTransform tm; - tf->lookupTransform(context->getFrameManager()->getFixedFrame(), planning_frame, ros::Time(), tm); - auto q = tm.getRotation(); - auto p = tm.getOrigin(); - quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z()); - pos = Ogre::Vector3(p.x(), p.y(), p.z()); - } else { - Q_ASSERT(data.first->header.frame_id == planning_frame); - } - data.second->setOrientation(quat * data.second->getOrientation()); - data.second->setPosition(quat * (data.second->getPosition() - pos)); + try { + tf::TransformListener* tf = context->getFrameManager()->getTFClient(); + std::string error_msg; + if (!tf->canTransform(planning_frame_, fixed_frame, ros::Time(), &error_msg)) { + ROS_WARN_STREAM_NAMED("MarkerVisualization", error_msg); + return false; // frame transform not (yet) available } + tf::StampedTransform tm; + tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm); + auto q = tm.getRotation(); + auto p = tm.getOrigin(); + quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z()); + pos = Ogre::Vector3(p.x(), p.y(), p.z()); + } catch (const tf2::TransformException& e) { + ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what()); + return false; } + + for (MarkerData& data : markers_) { + if (data.marker_) continue; + + auto ns_it = namespaces_.find(data.msg_->ns); + Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before! + if (ns_it->second.ns_node_ == nullptr) // create scene node for this namespace + ns_it->second.ns_node_ = parent_scene_node->getCreator()->createSceneNode(); + Ogre::SceneNode* node = ns_it->second.ns_node_; + + // create a scene node for all markers with given frame name + auto frame_it = ns_it->second.frames_.insert(std::make_pair(data.msg_->header.frame_id, nullptr)).first; + if (frame_it->second == nullptr) + frame_it->second = node->createChildSceneNode(); + node = frame_it->second; + + data.marker_.reset(createMarker(data.msg_->type, context, node)); + if (!data.marker_) continue; // failed to create marker + + // setMessage() initializes the marker, placing it at the message-specified frame + // w.r.t. rviz' current fixed frame. However, we want to place the marker w.r.t. + // the planning frame of the planning scene! + + // Hence, temporarily modify the message-specified frame to planning_frame_ + const std::string msg_frame = data.msg_->header.frame_id; + data.msg_->header.frame_id = planning_frame_; + data.marker_->setMessage(data.msg_); + data.msg_->header.frame_id = msg_frame; + + // ... and subsequently revert any transform between rviz' fixed frame and planning_frame_ + data.marker_->setOrientation(quat * data.marker_->getOrientation()); + data.marker_->setPosition(quat * data.marker_->getPosition() + pos); + } + markers_created_ = true; + return true; +} + +void MarkerVisualization::update(MarkerData& data, + const planning_scene::PlanningScene &scene, + const moveit::core::RobotState &robot_state) const +{ + Q_ASSERT(scene.getPlanningFrame() == planning_frame_); + + const visualization_msgs::Marker& marker = *data.msg_; + if (marker.header.frame_id == scene.getPlanningFrame()) + return; // no need to transform nodes placed at planning frame + + // fetch base pose from robot_state / scene + Eigen::Affine3d pose; + if (robot_state.knowsFrameTransform(marker.header.frame_id)) + pose = robot_state.getFrameTransform(marker.header.frame_id); + else if (scene.knowsFrameTransform(marker.header.frame_id)) + pose = scene.getFrameTransform(marker.header.frame_id); + else { + ROS_WARN_ONCE_NAMED("MarkerVisualization", + "unknown frame '%s' for solution marker in namespace '%s'", + marker.header.frame_id.c_str(), marker.ns.c_str()); + return; // ignore markers with unknown frame + } + + auto ns_it = namespaces_.find(marker.ns); + Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before + auto frame_it = ns_it->second.frames_.find(marker.header.frame_id); + Q_ASSERT(frame_it != ns_it->second.frames_.end()); // we have created all of them + + const Eigen::Quaterniond q = (Eigen::Quaterniond)pose.linear(); + const Eigen::Vector3d& p = pose.translation(); + frame_it->second->setOrientation(Ogre::Quaternion(q.w(), q.x(), q.y(), q.z())); + frame_it->second->setPosition(Ogre::Vector3(p.x(), p.y(), p.z())); +} + +void MarkerVisualization::update(const planning_scene::PlanningScene &end_scene, + const moveit::core::RobotState &robot_state) +{ + for (MarkerData& data : markers_) + update(data, end_scene, robot_state); } MarkerVisualizationProperty::MarkerVisualizationProperty(const QString &name, rviz::Property *parent) : rviz::BoolProperty(name, true, "Enable/disable markers", parent) { + all_markers_at_once_ = new rviz::BoolProperty("All at once?", false, "Show all markers of multiple subsolutions at once?", + this, SLOT(onAllAtOnceChanged()), this); + connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged())); } @@ -194,24 +250,42 @@ void MarkerVisualizationProperty::addMarkers(MarkerVisualizationPtr markers) // remember that those markers are hosted hosted_markers_.push_back(markers); + // create markers if not yet done + if (!markers->created() && !markers->createMarkers(context_, marker_scene_node_)) + return; // if markers not created, nothing to do here // attach all scene nodes from markers for (const auto& pair : markers->namespaces()) { + QString ns = QString::fromStdString(pair.first); // create sub property for newly encountered namespace, enabling visibility by default - auto ns_it = namespaces_.insert(std::make_pair(pair.first, nullptr)).first; + auto ns_it = namespaces_.insert(std::make_pair(ns, nullptr)).first; if (ns_it->second == nullptr) { - ns_it->second = new rviz::BoolProperty(pair.first, true, "Show/hide markers of this namespace", this, + ns_it->second = new rviz::BoolProperty(ns, true, "Show/hide markers of this namespace", this, SLOT(onNSEnableChanged()), this); } - if (!pair.second) // invalid scene node indicates that we still need to create the rviz markers - markers->createMarkers(context_, marker_scene_node_); - Q_ASSERT(pair.second); + Q_ASSERT(pair.second.ns_node_); // nodes should have been created in createMarkers() if (ns_it->second->getBool()) - marker_scene_node_->addChild(pair.second); + marker_scene_node_->addChild(pair.second.ns_node_); } } +void MarkerVisualizationProperty::update(const planning_scene::PlanningScene &scene, + const moveit::core::RobotState &robot_state) +{ + for (const auto& markers : hosted_markers_) { + if (!markers->created()) + if (!markers->createMarkers(context_, marker_scene_node_)) + continue; + markers->update(scene, robot_state); + } +} + +bool MarkerVisualizationProperty::allAtOnce() const +{ + return all_markers_at_once_->getBool(); +} + void MarkerVisualizationProperty::onEnableChanged() { setVisibility(marker_scene_node_, parent_scene_node_, getBool()); @@ -227,4 +301,9 @@ void MarkerVisualizationProperty::onNSEnableChanged() markers->setVisible(ns, marker_scene_node_, visible); } +void MarkerVisualizationProperty::onAllAtOnceChanged() +{ + Q_EMIT allAtOnceChanged(all_markers_at_once_->getBool()); +} + } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 5560ea7a..2b8e327b 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -152,6 +153,9 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR); octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR); + + marker_visual_ = new MarkerVisualizationProperty("Markers", parent); + connect(marker_visual_, SIGNAL(allAtOnceChanged(bool)), this, SLOT(onAllAtOnceChanged(bool))); } TaskSolutionVisualization::~TaskSolutionVisualization() @@ -165,6 +169,8 @@ TaskSolutionVisualization::~TaskSolutionVisualization() if (slider_dock_panel_) delete slider_dock_panel_; + delete marker_visual_; + if (main_scene_node_) main_scene_node_->getCreator()->destroySceneNode(main_scene_node_); } @@ -188,6 +194,8 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz:: scene_render_.reset(new PlanningSceneRender(main_scene_node_, context_, RobotStateVisualizationPtr())); scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool()); + marker_visual_->onInitialize(main_scene_node_, context_); + rviz::WindowManagerInterface* window_context = context_->getWindowManager(); if (window_context) { @@ -330,6 +338,18 @@ void TaskSolutionVisualization::interruptCurrentDisplay() drop_displaying_solution_ = true; } +void TaskSolutionVisualization::onAllAtOnceChanged(bool all_at_once) +{ + if (!displaying_solution_) + return; + clearMarkers(); + + if (all_at_once) + addMarkers(displaying_solution_); + else if (current_state_ >= 0) + renderWayPoint(current_state_, -1); +} + float TaskSolutionVisualization::getStateDisplayTime() { std::string tm = state_display_time_property_->getStdString(); @@ -458,6 +478,11 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) displaying_solution_->indexPair(previous_index).first != idx_pair.first) { // switch to new stage: show new planning scene renderPlanningScene (scene); + // switch to markers of next sub trajectory? + if (!marker_visual_->allAtOnce()) { + marker_visual_->clearMarkers(); + marker_visual_->addMarkers(displaying_solution_->markersOfSubTrajectory(idx_pair.first)); + } Q_EMIT activeStageChanged(displaying_solution_->creatorId(idx_pair)); } robot_state = displaying_solution_->getWayPointPtr(idx_pair); @@ -473,6 +498,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) planning_scene::ObjectColorMap color_map; scene->getKnownObjectColors(color_map); robot_render_->update(robot_state, color, color_map); + marker_visual_->update(*scene, *robot_state); if (slider_panel_ && index >= 0) slider_panel_->setSliderPosition(index); @@ -502,7 +528,7 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msg showTrajectory(s, false); } -void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s, bool lock_display) +void TaskSolutionVisualization::showTrajectory(const DisplaySolutionPtr& s, bool lock_display) { if (lock_display || !s->empty()) { boost::mutex::scoped_lock lock(display_solution_mutex_); @@ -516,7 +542,22 @@ void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s, bool lock_d void TaskSolutionVisualization::unlock() { - locked_ = false; + locked_ = false; +} + +void TaskSolutionVisualization::clearMarkers() +{ + marker_visual_->clearMarkers(); +} + +void TaskSolutionVisualization::addMarkers(const moveit_rviz_plugin::DisplaySolutionPtr &s) +{ + if (!s || (!marker_visual_->allAtOnce() && s->numSubSolutions() > 1)) + return; + + for (size_t i = 0, end = s->numSubSolutions(); i != end; ++i) { + marker_visual_->addMarkers(s->markersOfSubTrajectory(i)); + } } void TaskSolutionVisualization::changedRobotColor()