Added Bt LED node

src/msgs/CMakeLists.txt

@@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/JointStatus.msg"
   "msg/SystemInfo.msg"
   "msg/Heading.msg"
+  "msg/LedStatus.msg"
   "srv/SetController.srv"
   "srv/StatusReq.srv"
   "srv/MaintenanceReq.srv"

src/subsystems/navigation/athena_planner/CMakeLists.txt

@@ -15,13 +15,15 @@ find_package(nav_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(vision_msgs REQUIRED)
+find_package(msgs REQUIRED)
 
 add_library(bt_nodes SHARED
   plugins/nav_selector_node.cpp
   plugins/aruco_detected_node.cpp
   plugins/get_aruco_pose_node.cpp
   plugins/spiral_coverage_action_node.cpp
   plugins/object_detected_node.cpp
+  plugins/led_status_node.cpp
 )
 
 target_compile_definitions(bt_nodes PRIVATE BT_PLUGIN_EXPORT)
@@ -42,6 +44,7 @@ ament_target_dependencies(bt_nodes
   tf2
   tf2_geometry_msgs
   vision_msgs
+  msgs
 )
 
 install(TARGETS bt_nodes

src/subsystems/navigation/athena_planner/behavior_trees/aruco_detection_navigation.xml

@@ -22,6 +22,7 @@
             </Sequence>
           </ReactiveFallback>
         </RecoveryNode> 
+        <LedStatus color="green" topic_name="/led_status" />
       </Sequence>
 
       <!-- ArUco NOT detected - execute spiral coverage search -->

src/subsystems/navigation/athena_planner/behavior_trees/gnss_navigation.xml

@@ -1,10 +1,14 @@
 <root>
   <BehaviorTree ID="GNSSNavigation">
-    <PipelineSequence name="NavigateWithReplanning">
-      <RateController hz="5.0">
-        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
-      </RateController>
-      <FollowPath path="{path}" controller_id="FollowPath"/>
-    </PipelineSequence>
+    <Sequence> 
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="5.0">
+          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+        </RateController>
+        <FollowPath path="{path}" controller_id="FollowPath"/>
+      </PipelineSequence>
+
+      <LedStatus color="green" topic_name="/led_status" />
+    </Sequence>
   </BehaviorTree>
 </root>

src/subsystems/navigation/athena_planner/behavior_trees/main_router.xml

@@ -10,6 +10,8 @@
 
       <NavSelector topic_name="/nav_mode" default_nav_mode="GNSS" nav_mode="{nav_mode}" />
 
+      <LedStatus color="red" topic_name="/led_status" />
+
       <ReactiveFallback name="SelectMode">
 
         <BlackboardCheckString value_A="{nav_mode}" value_B="GNSS" return_on_mismatch="FAILURE">

src/subsystems/navigation/athena_planner/behavior_trees/object_detection_navigation.xml

@@ -5,6 +5,7 @@
       <!-- Sequence 1: Check if object is detected -->
       <Sequence name="ObjectDetectedSequence">
         <ObjectDetected object_topic="/yolo/target_found" timeout="1.0" />
+        <LedStatus color="green" topic_name="/led_status" />
       </Sequence>
 
       <!-- Sequence 2: If not detected, perform spiral search -->

src/subsystems/navigation/athena_planner/include/athena_planner/led_status_node.hpp

@@ -0,0 +1,41 @@
+#ifndef ATHENA_PLANNER__LED_STATUS_NODE_HPP_
+#define ATHENA_PLANNER__LED_STATUS_NODE_HPP_
+
+#include <string>
+
+#include "behaviortree_cpp_v3/action_node.h"
+#include "rclcpp/rclcpp.hpp"
+
+// Change this include if your msg package name is different
+#include "msgs/msg/led_status.hpp"
+
+namespace bt_nodes
+{
+
+class LedStatusNode : public BT::SyncActionNode
+{
+public:
+  LedStatusNode(
+    const std::string & name,
+    const BT::NodeConfiguration & conf);
+
+  BT::NodeStatus tick() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::string>("color"),
+      BT::InputPort<std::string>("topic_name", "/led_status")
+    };
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Publisher<msgs::msg::LedStatus>::SharedPtr led_pub_;
+
+  std::string topic_name_;
+};
+
+}  // namespace bt_nodes
+
+#endif  // ATHENA_PLANNER__LED_STATUS_NODE_HPP_

src/subsystems/navigation/athena_planner/plugins/led_status_node.cpp

@@ -0,0 +1,67 @@
+#include "athena_planner/led_status_node.hpp"
+
+namespace bt_nodes
+{
+
+LedStatusNode::LedStatusNode(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  topic_name_ = "/led_status";
+  getInput("topic_name", topic_name_);
+
+  led_pub_ = node_->create_publisher<msgs::msg::LedStatus>(
+    topic_name_,
+    10);
+}
+
+BT::NodeStatus LedStatusNode::tick()
+{
+  std::string color;
+  if (!getInput("color", color)) {
+    RCLCPP_ERROR(node_->get_logger(), "LedStatusNode missing required input port [color]");
+    return BT::NodeStatus::FAILURE;
+  }
+
+  msgs::msg::LedStatus msg;
+  msg.cmd = msgs::msg::LedStatus::CMD_SOLID;
+  msg.r = 0;
+  msg.g = 0;
+  msg.b = 0;
+  msg.param = 0;
+
+  if (color == "red" || color == "RED") {
+    msg.cmd = msgs::msg::LedStatus::CMD_SOLID;
+    msg.r = 255;
+    msg.g = 0;
+    msg.b = 0;
+  } else if (color == "green" || color == "GREEN") {
+    msg.cmd = msgs::msg::LedStatus::CMD_FLASH;
+    msg.r = 0;
+    msg.g = 255;
+    msg.b = 0;
+  } else {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "Invalid LED color [%s]. Expected red or green.",
+      color.c_str());
+    return BT::NodeStatus::FAILURE;
+  }
+
+  led_pub_->publish(msg);
+
+  RCLCPP_INFO(
+    node_->get_logger(),
+    "Published LED status: color=%s rgb=[%u, %u, %u]",
+    color.c_str(),
+    msg.r,
+    msg.g,
+    msg.b);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace bt_nodes