diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp index 7c6c341cfd927a65d5ea6cf06469431d416a83f1..dd7d9ea9e713332d95781be092a87608a66b6f21 100644 --- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp +++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp @@ -300,15 +300,24 @@ armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop() StringVariantBaseMap m; m["err_dist"] = new Variant{vels.err_dist}; m["err_angular_dist"] = new Variant{vels.err_angular_dist}; + m["target_global_x"] = new Variant{vels.target_global.x()}; m["target_global_y"] = new Variant{vels.target_global.y()}; + m["target_global_abs"] = new Variant{vels.target_global.norm()}; + m["target_local_x"] = new Variant{vels.target_local.x()}; m["target_local_y"] = new Variant{vels.target_local.y()}; + m["target_local_abs"] = new Variant(vels.target_local.norm()); m["target_rot"] = new Variant{vels.target_rot}; + m["modulated_global_x"] = new Variant{vels.modulated_global.x()}; m["modulated_global_y"] = new Variant{vels.modulated_global.y()}; + m["modulated_global_abs"] = new Variant{vels.modulated_global.norm()}; + m["modulated_local_x"] = new Variant{vels.modulated_local.x()}; m["modulated_local_y"] = new Variant{vels.modulated_local.y()}; + m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()}; + setDebugObserverChannel("ObstacleAvoidingPlatformCtrl", m); m_platform->move(vels.modulated_local.x(), vels.modulated_local.y(), vels.target_rot);