diff --git a/mujoco_ros2_control_plugins/README.md b/mujoco_ros2_control_plugins/README.md index 9e2b1879..fa0d9d4f 100644 --- a/mujoco_ros2_control_plugins/README.md +++ b/mujoco_ros2_control_plugins/README.md @@ -55,8 +55,8 @@ attachment position. | Key | Action | |---|---| | `G` | Toggle gantry on / off. Re-enabling re-anchors above the current attachment position | -| `[` | Shorten rope by 0.5 cm | -| `]` | Lengthen rope by 0.5 cm | +| `[` | Shorten rope by 0.5 cm *(ignored with a warning when gantry is disabled)* | +| `]` | Lengthen rope by 0.5 cm *(ignored with a warning when gantry is disabled)* | #### ROS 2 interface diff --git a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp index ac5c0e31..5196b5a7 100644 --- a/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/virtual_gantry_plugin.cpp @@ -119,8 +119,15 @@ void VirtualGantryPlugin::update(const mjModel* /*model*/, mjData* data) const int scroll_ticks = rope_length_ticks_.exchange(0, std::memory_order_acquire); if (scroll_ticks != 0) { - rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.005); - RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: rope_length=%.3f m", rope_length_); + if (enabled_) + { + rope_length_ = std::max(0.1, rope_length_ + scroll_ticks * 0.005); + RCLCPP_INFO(node_->get_logger(), "VirtualGantryPlugin: rope_length=%.3f m", rope_length_); + } + else + { + RCLCPP_WARN(node_->get_logger(), "VirtualGantryPlugin: gantry disabled — rope length unchanged"); + } } // --- Compute attachment point in world frame ------------------------------