Feature: Visualization tweaks.

source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp

@@ -300,6 +300,8 @@ armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
     m_platform->stopPlatform();
     m_control_loop.mode = control_mode::none;
 
+    visualize();
+
     ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
 }
 
@@ -628,6 +630,21 @@ const
 }
 
 
+void
+armarx::ObstacleAvoidingPlatformUnit::visualize()
+{
+    const Eigen::Vector2f zero = Eigen::Vector2f::Zero();
+    velocities vels;
+    vels.target_local = zero;
+    vels.target_global = zero;
+    vels.modulated_local = zero;
+    vels.modulated_global = zero;
+    vels.target_rot = 0;
+
+    visualize(vels);
+}
+
+
 void
 armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
 {
@@ -699,12 +716,13 @@ armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
     // Velocities.
     Layer l_vels = arviz.layer("velocities");
     {
+        const float min_velocity = 15;
         const Eigen::Vector3f from1{agent_pos + Eigen::Vector3f{0, 0, 2000}};
         const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
         const Eigen::Vector3f original{vels.target_global.x(), vels.target_global.y(), 0};
         const Eigen::Vector3f modulated{vels.modulated_global.x(), vels.modulated_global.y(), 0};
 
-        if (not original.isZero())
+        if (original.norm() > min_velocity)
         {
             l_vels.add(Arrow{"original"}
                        .fromTo(from1, from1 + original * 5)
@@ -712,7 +730,7 @@ armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
                        .width(25));
         }
 
-        if (not modulated.isZero())
+        if (modulated.norm() > min_velocity)
         {
             l_vels.add(Arrow{"modulated"}
                        .fromTo(from2, from2 + modulated * 5)

source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h

@@ -246,6 +246,9 @@ namespace armarx
         calculate_adaptive_max_vel()
         const;
 
+        void
+        visualize();
+
         void
         visualize(const velocities& vels);