Lights auto mode (#610)

* Add twist mux controller

* Use source of velocity command for lights system

* remove joy subscriber

* update docs

* add input check

* remove unused variable

Author: KmakD

husarion_ugv_lights/CONFIGURATION.md

@@ -116,7 +116,7 @@ Animation of type `husarion_ugv_lights::MovingImageAnimation`, returns frames to
   <img alt="MovingImageAnimation" src=".docs/MovingImageAnimation.png" />
 </picture>
 
-### Defining Animations
+## Defining Animations
 
 Users can define their own LED animations using basic animation types. Similar to basic ones, user animations are parsed using YAML file and loaded on node start. For `ImageAnimation` you can use basic images from the `animations` folder and change their color with the `color` key ([see ImageAnimation](#imageanimation)). Follow the example below to add custom animations.
 

husarion_ugv_manager/CMakeLists.txt

@@ -58,6 +58,10 @@ add_library(check_joy_msg_bt_node SHARED
             src/plugins/condition/check_joy_msg.cpp)
 list(APPEND plugin_libs check_joy_msg_bt_node)
 
+add_library(check_string_msg_bt_node SHARED
+            src/plugins/condition/check_string_msg.cpp)
+list(APPEND plugin_libs check_string_msg_bt_node)
+
 add_library(tick_after_timeout_bt_node SHARED
             src/plugins/decorator/tick_after_timeout_node.cpp)
 list(APPEND plugin_libs tick_after_timeout_bt_node)
@@ -163,6 +167,12 @@ if(BUILD_TESTING)
     src/plugins/condition/check_joy_msg.cpp)
   list(APPEND plugin_tests ${PROJECT_NAME}_test_check_joy_msg)
 
+  ament_add_gtest(
+    ${PROJECT_NAME}_test_check_string_msg
+    test/plugins/condition/test_check_string_msg.cpp
+    src/plugins/condition/check_string_msg.cpp)
+  list(APPEND plugin_tests ${PROJECT_NAME}_test_check_string_msg)
+
   ament_add_gtest(
     ${PROJECT_NAME}_test_tick_after_timeout_node
     test/plugins/decorator/test_tick_after_timeout_node.cpp

husarion_ugv_manager/README.md

@@ -31,6 +31,7 @@ Node responsible for managing Bumper Lights animation scheduling.
 
 - `battery/battery_status` [*sensor_msgs/BatteryState*]: State of the internal Battery.
 - `hardware/e_stop` [*std_msgs/Bool*]: State of emergency stop.
+- `twist_mux_controller/source` [*std_msgs/String]: Source of the command velocity.
 
 #### Service Clients (for Default Trees)
 

husarion_ugv_manager/behavior_trees/LightsBT.btproj

@@ -9,6 +9,10 @@
             <input_port name="repeating" default="false">indicates if animation should repeat</input_port>
             <input_port name="service_name">ROS service name</input_port>
         </Action>
+        <Condition ID="CheckStringMsg" editable="true">
+            <input_port name="topic_name">Topic name</input_port>
+            <input_port name="data">Specifies the expected state of the data field</input_port>
+        </Condition>
         <Decorator ID="TickAfterTimeout" editable="true">
             <input_port name="timeout">time in s to wait before ticking child again</input_port>
         </Decorator>

husarion_ugv_manager/behavior_trees/lights.xml

@@ -107,22 +107,35 @@
 
   <BehaviorTree ID="StateAnimationSubtree">
     <Sequence>
-      <Sequence _skipIf="e_stop_state">
-        <CallSetLedAnimationService name="SetReadyAnimation"
-                                    id="{READY_ANIM_ID}"
-                                    param=""
-                                    repeating="true"
-                                    service_name="lights/set_animation"
-                                    _skipIf="drive_state || current_anim_id == READY_ANIM_ID"
-                                    _onSuccess="current_anim_id = READY_ANIM_ID"/>
+      <IfThenElse _skipIf="e_stop_state">
+        <CheckStringMsg topic_name="twist_mux_controller/source"
+                        data="manual"/>
         <CallSetLedAnimationService name="SetManualActionAnimation"
                                     id="{MANUAL_ACTION_ANIM_ID}"
                                     param=""
                                     repeating="true"
                                     service_name="lights/set_animation"
-                                    _skipIf="(!drive_state) || current_anim_id == MANUAL_ACTION_ANIM_ID"
+                                    _skipIf="current_anim_id == MANUAL_ACTION_ANIM_ID"
                                     _onSuccess="current_anim_id = MANUAL_ACTION_ANIM_ID"/>
-      </Sequence>
+        <IfThenElse>
+          <CheckStringMsg topic_name="twist_mux_controller/source"
+                          data="autonomous"/>
+          <CallSetLedAnimationService name="SetManualActionAnimation"
+                                      id="{AUTONOMOUS_ACTION_ANIM_ID}"
+                                      param=""
+                                      repeating="true"
+                                      service_name="lights/set_animation"
+                                      _skipIf="current_anim_id == AUTONOMOUS_ACTION_ANIM_ID"
+                                      _onSuccess="current_anim_id = AUTONOMOUS_ACTION_ANIM_ID"/>
+          <CallSetLedAnimationService name="SetReadyAnimation"
+                                      id="{READY_ANIM_ID}"
+                                      param=""
+                                      repeating="true"
+                                      service_name="lights/set_animation"
+                                      _skipIf="current_anim_id == READY_ANIM_ID"
+                                      _onSuccess="current_anim_id = READY_ANIM_ID"/>
+        </IfThenElse>
+      </IfThenElse>
       <CallSetLedAnimationService name="SetEStopAnimation"
                                   id="{E_STOP_ANIM_ID}"
                                   param=""
@@ -143,6 +156,11 @@
                   default="false">indicates if animation should repeat</input_port>
       <input_port name="service_name">ROS service name</input_port>
     </Action>
+    <Condition ID="CheckStringMsg"
+               editable="true">
+      <input_port name="topic_name">Topic name</input_port>
+      <input_port name="data">Specifies the expected state of the data field</input_port>
+    </Condition>
     <Decorator ID="TickAfterTimeout"
                editable="true">
       <input_port name="timeout">time in s to wait before ticking child again</input_port>

husarion_ugv_manager/config/lights_manager.yaml

@@ -19,3 +19,4 @@
         - tick_after_timeout_bt_node
       ros_plugin_libs:
         - call_set_led_animation_service_bt_node
+        - check_string_msg_bt_node

husarion_ugv_manager/include/husarion_ugv_manager/lights_manager_node.hpp

@@ -23,7 +23,6 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "sensor_msgs/msg/battery_state.hpp"
-#include "sensor_msgs/msg/joy.hpp"
 #include "std_msgs/msg/bool.hpp"
 
 #include "husarion_ugv_msgs/msg/led_animation.hpp"
@@ -39,7 +38,6 @@ namespace husarion_ugv_manager
 using BatteryStateMsg = sensor_msgs::msg::BatteryState;
 using BoolMsg = std_msgs::msg::Bool;
 using LEDAnimationMsg = husarion_ugv_msgs::msg::LEDAnimation;
-using JoyMsg = sensor_msgs::msg::Joy;
 
 /**
  * @brief This class is responsible for creating a BehaviorTree responsible for lights management,
@@ -73,19 +71,15 @@ class LightsManagerNode : public rclcpp::Node
 private:
   void BatteryCB(const BatteryStateMsg::SharedPtr battery);
   void EStopCB(const BoolMsg::SharedPtr e_stop);
-  void JoyCB(const JoyMsg::SharedPtr joy);
   void LightsTreeTimerCB();
 
-  static constexpr std::size_t kDeadManButtonIndex = 4;
-
   float update_charging_anim_step_;
 
   std::shared_ptr<lights_manager::ParamListener> param_listener_;
   lights_manager::Params params_;
 
   rclcpp::Subscription<BatteryStateMsg>::SharedPtr battery_sub_;
   rclcpp::Subscription<BoolMsg>::SharedPtr e_stop_sub_;
-  rclcpp::Subscription<JoyMsg>::SharedPtr joy_sub_;
   rclcpp::TimerBase::SharedPtr lights_tree_timer_;
 
   std::unique_ptr<husarion_ugv_utils::MovingAverage<double>> battery_percent_ma_;

husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_string_msg.hpp

@@ -0,0 +1,56 @@
+// Copyright 2025 Husarion sp. z o.o.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_STRING_MSG_HPP_
+#define HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_STRING_MSG_HPP_
+
+#include <memory>
+#include <string>
+
+#include <behaviortree_ros2/bt_topic_sub_node.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <std_msgs/msg/string.hpp>
+
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+
+namespace husarion_ugv_manager
+{
+
+// FIXME: There is no possibility to set QoS profile. Add it in the future to subscribe e_stop.
+class CheckStringMsg : public BT::RosTopicSubNode<std_msgs::msg::String>
+{
+  using StringMsg = std_msgs::msg::String;
+
+public:
+  CheckStringMsg(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosTopicSubNode<StringMsg>(name, conf, params)
+  {
+  }
+
+  BT::NodeStatus onTick(const StringMsg::SharedPtr & last_msg) override;
+
+  bool latchLastMessage() const override;
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {BT::InputPort<std::string>("data", "Specifies the expected state of the data field.")});
+  }
+};
+
+}  // namespace husarion_ugv_manager
+
+#endif  // HUSARION_UGV_MANAGER_HUSARION_UGV_MANAGER_PLUGINS_CONDITION_CHECK_STRING_MSG_HPP_

husarion_ugv_manager/src/lights_manager_node.cpp

@@ -24,7 +24,6 @@
 
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/joy.hpp"
 
 #include "husarion_ugv_utils/moving_average.hpp"
 
@@ -72,8 +71,6 @@ void LightsManagerNode::Initialize()
   e_stop_sub_ = this->create_subscription<BoolMsg>(
     "hardware/e_stop", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&LightsManagerNode::EStopCB, this, _1));
-  joy_sub_ = this->create_subscription<JoyMsg>(
-    "joy", 10, std::bind(&LightsManagerNode::JoyCB, this, _1));
 
   const double timer_freq = this->params_.timer_frequency;
   const auto timer_period = std::chrono::duration<double>(1.0 / timer_freq);
@@ -126,7 +123,6 @@ std::map<std::string, std::any> LightsManagerNode::CreateLightsInitialBlackboard
     {"current_anim_id", undefined_anim_id},
     {"current_battery_anim_id", undefined_anim_id},
     {"current_error_anim_id", undefined_anim_id},
-    {"drive_state", false},
     {"CRITICAL_BATTERY_THRESHOLD_PERCENT", critical_battery_threshold_percent},
     {"LOW_BATTERY_ANIM_PERIOD", low_battery_anim_period},
     {"LOW_BATTERY_THRESHOLD_PERCENT", low_battery_threshold_percent},
@@ -190,12 +186,6 @@ void LightsManagerNode::EStopCB(const BoolMsg::SharedPtr e_stop)
   lights_tree_manager_->GetBlackboard()->set<bool>("e_stop_state", e_stop->data);
 }
 
-void LightsManagerNode::JoyCB(const JoyMsg::SharedPtr joy)
-{
-  lights_tree_manager_->GetBlackboard()->set<bool>(
-    "drive_state", joy->buttons[kDeadManButtonIndex]);
-}
-
 void LightsManagerNode::LightsTreeTimerCB()
 {
   if (!SystemReady()) {

husarion_ugv_manager/src/plugins/condition/check_string_msg.cpp

@@ -0,0 +1,42 @@
+// Copyright 2025 Husarion sp. z o.o.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "husarion_ugv_manager/plugins/condition/check_string_msg.hpp"
+
+#include <memory>
+#include <string>
+
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"
+
+namespace husarion_ugv_manager
+{
+
+BT::NodeStatus CheckStringMsg::onTick(const StringMsg::SharedPtr & last_msg)
+{
+  std::string expected_data;
+  if (!getInput<std::string>("data", expected_data)) {
+    RCLCPP_ERROR_STREAM(this->logger(), GetLoggerPrefix(name()) << "Failed to get input [data]");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  return (last_msg && last_msg->data == expected_data) ? BT::NodeStatus::SUCCESS
+                                                       : BT::NodeStatus::FAILURE;
+}
+
+bool CheckStringMsg::latchLastMessage() const { return true; }
+
+}  // namespace husarion_ugv_manager
+
+#include "behaviortree_ros2/plugins.hpp"
+CreateRosNodePlugin(husarion_ugv_manager::CheckStringMsg, "CheckStringMsg");