switch arviz to visualization helper in ReachabilityFilter

source/armarx/manipulation/action_planning/planning_steps/filtering/ReachabilityFilter.cpp

@@ -43,14 +43,15 @@
 
 namespace armarx::manipulation::planning::steps
 {
-    ReachabilityFilter::ReachabilityFilter(const std::string& name,
-                                           std::shared_ptr<VirtualRobot::Robot> robot,
-                                           const arondto::config::ReachabilityFilter& config,
-                                           const std::optional<viz::Client>& arvizClient) :
+    ReachabilityFilter::ReachabilityFilter(
+        const std::string& name,
+        std::shared_ptr<VirtualRobot::Robot> robot,
+        const arondto::config::ReachabilityFilter& config,
+        const std::experimental::observer_ptr<util::VisualizationHelper>& vizHelper) :
         AsyncPlanningStep(name),
         robot_(std::move(robot)),
         diffIkParameters_{},
-        arvizClient_(arvizClient)
+        arvizHelper_(vizHelper)
     {
         diffIkParameters_.ikStepLengthInitial = config.diffIkParams.ikStepLengthInitial;
         diffIkParameters_.ikStepLengthFineTune = config.diffIkParams.ikStepLengthFineTune;
@@ -72,9 +73,9 @@ namespace armarx::manipulation::planning::steps
             ARMARX_VERBOSE << "Action is not reachable, discarding it";
             action.markDiscarded();
         }
-        if (arvizClient_.has_value())
+        if (arvizHelper_ != nullptr)
         {
-            arvizClient_->commitDeleteLayer("reachability_filter");
+            arvizHelper_->clearLayer("Robot");
         }
     }
 
@@ -117,8 +118,8 @@ namespace armarx::manipulation::planning::steps
         { return pose.toGlobalEigen(robotCopy); };
 
         std::size_t current_pose = 0;
-        auto isPoseReachable =
-            [robotCopy, rnsName, tcpName, ikParams, &current_pose, this](const Eigen::Matrix4f& globalPose) -> bool
+        auto isPoseReachable = [robotCopy, rnsName, tcpName, ikParams, &current_pose, this](
+                                   const Eigen::Matrix4f& globalPose) -> bool
         {
             // Calculate the IK of the target in the local frame of the robot_ at the platform pose
             auto localRobotCopy = robotCopy->clone();
@@ -128,16 +129,10 @@ namespace armarx::manipulation::planning::steps
                 localRobotCopy->getRobotNodeSet(rnsName),
                 localRobotCopy->getRobotNode(tcpName),
                 ikParams);
-            if (arvizClient_.has_value())
+            if (arvizHelper_ != nullptr)
             {
-                viz::Robot robot(std::to_string(current_pose++));
-                // TODO: create convenience function for this in ArViz
-                robot.file("", localRobotCopy->getFilename())
-                    .pose(localRobotCopy->getGlobalPose())
-                    .joints(localRobotCopy->getJointValues())
-                    .overrideColor(result.reached ? armarx::viz::Color::green()
-                                                  : armarx::viz::Color::red());
-                arvizClient_->commitLayerContaining(getName(), robot);
+                arvizHelper_->visualizeRobot(
+                    localRobotCopy, result.reached ? simox::Color::green() : simox::Color::red());
             }
             return result.reached;
         };
@@ -153,14 +148,15 @@ namespace armarx::manipulation::planning::steps
         return ranges::all_of(globalPoses, isPoseReachable);
     }
 
-    ReachabilityFilter::ReachabilityFilter(const std::string& name,
-                                           std::shared_ptr<VirtualRobot::Robot> robot,
-                                           const SimpleDiffIK::Parameters& diffIkParameters,
-                                           const std::optional<viz::Client>& arvizClient) :
+    ReachabilityFilter::ReachabilityFilter(
+        const std::string& name,
+        std::shared_ptr<VirtualRobot::Robot> robot,
+        const SimpleDiffIK::Parameters& diffIkParameters,
+        const std::experimental::observer_ptr<util::VisualizationHelper>& vizHelper) :
         AsyncPlanningStep(name),
         robot_(std::move(robot)),
         diffIkParameters_(diffIkParameters),
-        arvizClient_(arvizClient)
+        arvizHelper_(vizHelper)
     {
     }
 } // namespace armarx::manipulation::planning::steps

source/armarx/manipulation/action_planning/planning_steps/filtering/ReachabilityFilter.h

@@ -28,6 +28,7 @@
 #include <armarx/manipulation/action_planning/PlanningStep.h>
 #include <armarx/manipulation/action_planning/planning_steps/aron/ReachabilityFilterConfig.aron.generated.h>
 #include <armarx/manipulation/core/forward_declarations.h>
+#include <armarx/manipulation/util/VisualizationHelper.h>
 
 namespace armarx::manipulation::planning::steps
 {
@@ -38,11 +39,11 @@ namespace armarx::manipulation::planning::steps
         ReachabilityFilter(const std::string& name,
                            std::shared_ptr<VirtualRobot::Robot> robot,
                            const arondto::config::ReachabilityFilter& diffIkParameters,
-                           const std::optional<viz::Client>& arvizClient = std::nullopt);
+                           const std::experimental::observer_ptr<util::VisualizationHelper>& vizHelper = nullptr);
         ReachabilityFilter(const std::string& name,
                            std::shared_ptr<VirtualRobot::Robot> robot,
                            const armarx::SimpleDiffIK::Parameters& diffIkParameters,
-                           const std::optional<viz::Client>& arvizClient = std::nullopt);
+                           const std::experimental::observer_ptr<util::VisualizationHelper>& vizHelper = nullptr);
 
         void execute(core::ExecutableAction& action) const override;
 
@@ -55,6 +56,6 @@ namespace armarx::manipulation::planning::steps
 
         const std::shared_ptr<VirtualRobot::Robot> robot_;
         armarx::SimpleDiffIK::Parameters diffIkParameters_;
-        mutable std::optional<viz::Client> arvizClient_;
+        std::experimental::observer_ptr<util::VisualizationHelper> arvizHelper_;
     };
 } // namespace armarx::manipulation::planning::steps

source/armarx/manipulation/util/VisualizationHelper.cpp

@@ -97,14 +97,18 @@ namespace armarx::manipulation::util
     }
 
     void
-    VisualizationHelper::visualizeRobot(const VirtualRobot::RobotPtr& robot)
+    VisualizationHelper::visualizeRobot(const VirtualRobot::RobotPtr& robot,
+                                        const std::optional<simox::Color>& colorOverride)
     {
         const auto& config = configPlugin_->config_.getUpToDateReadBuffer();
         auto vis_robot = armarx::viz::Robot("Robot")
                              .file(config.robot_project_dir, config.robot_path)
                              .joints(robot->getConfig()->getRobotNodeJointValueMap())
-                             .pose(robot->getGlobalPose())
-                             .overrideColor(simox::Color::blue());
+                             .pose(robot->getGlobalPose());
+        if (colorOverride)
+        {
+            vis_robot.overrideColor(colorOverride.value());
+        }
         arviz_.commitLayerContaining("Robot", vis_robot);
     }
 
@@ -132,7 +136,6 @@ namespace armarx::manipulation::util
     void
     VisualizationHelper::clearVisualizationLayers()
     {
-        arviz_.commitDeleteLayer("MovementInstructions");
         arviz_.commitDeleteLayer("Robot");
         arviz_.commitDeleteLayer("Hypotheses");
         arviz_.commitDeleteLayer("Trajectory");
@@ -174,6 +177,28 @@ namespace armarx::manipulation::util
         }
     }
 
+    void
+    VisualizationHelper::clearLayer(const std::string& layerName)
+    {
+        if (std::equal(layerName.begin(), layerName.end(), "Hypotheses"))
+        {
+            arviz_.commitDeleteLayer("Hypotheses");
+        }
+        else if (std::equal(layerName.begin(), layerName.end(), "Trajectory"))
+        {
+            arviz_.commitDeleteLayer("Trajectory");
+        }
+        else if (std::equal(layerName.begin(), layerName.end(), "Robot"))
+        {
+            arviz_.commitDeleteLayer("Robot");
+        }
+        else
+        {
+            // do nothing
+            return;
+        }
+    }
+
     template <typename PluginT, typename... ParamsT>
     void
     VisualizationHelper::addPluginAndDependency(std::experimental::observer_ptr<PluginT>& plugin,

source/armarx/manipulation/util/VisualizationHelper.h

@@ -74,7 +74,8 @@ namespace armarx::manipulation::util
         /**
          * \brief Visualizes the local robot clone (with commiting the Layer)
          */
-        void visualizeRobot(const VirtualRobot::RobotPtr& robot);
+        void visualizeRobot(const VirtualRobot::RobotPtr& robot,
+                            const std::optional<simox::Color>& colorOverride = std::nullopt);
 
         /**!
          * \brief visualize the trajectoy without synching the robot first
@@ -88,6 +89,7 @@ namespace armarx::manipulation::util
 
 
         void clearVisualizationLayers();
+        void clearLayer(const std::string& layerName);
 
 
         armarx::viz::Client& getArvizClient();