Feat/change controller type by 2006wu · Pull Request #9 · DIT-ROBOTICS/Eurobot-2026-Main

2006wu · 2026-04-05T11:25:05Z

Add a node ControllerTypePublisher letting the the main program able to change the msg in /controller_type when the robot is running.

It has two input: mode & controller_type

Mode

Auto

the node will check the sequence of pantry and collection, if both sequences are empty, then it will change the /controller_type to the target one

Manual

the node will change the /controller_type directly to what you want

How to use it ?

I put it in the TakeSubTree like this

 <BehaviorTree ID="TakeSubTree">
      <Sequence>
          <ControllerTypePublisher
              mode="Auto"
              controller_type="Fast"/>
          <!-- Dock to collection pdoint -->
...

It means that my target /controller_type is Fast, and if the pantry sequence and collection sequence are empty, then it will change the /controller_type to Fast.

I also put it in the GoHomeSubTree like this

<ControllerTypePublisher
    mode="Auto"
    controller_type="Slow"/>
...

It means I want to change the /controller_type to Slow directly.

Copilot

Pull request overview

This PR introduces a BehaviorTree node intended to switch the active “controller type” at runtime and uses that state to select different docking controller IDs (fast/slow/normal) when generating DockRobot goals.

Changes:

Added ControllerTypePublisher BT node and registered it in bt_engine + node model XML.
Updated OnDockAction to choose dock controller IDs based on a blackboard controller_type (Fast/Slow/Normal) and added corresponding parameters.
Updated robot configs and BT XML plans to provide fast/slow dock types and insert the new node into execution flow.

Reviewed changes

Copilot reviewed 14 out of 14 changed files in this pull request and generated 10 comments.

Show a summary per file

File	Description
src/startup/params/robot_config_white.yaml	Updates `default_plan` for white robot config.
src/startup/params/robot_config_black.yaml	Adds fast/slow dock controller ID parameters.
src/navigation/src/onDock.cpp	Selects dock controller ID based on blackboard `controller_type`; declares/loads fast/slow params.
src/navigation/include/onDock.hpp	Adds member fields for fast/slow dock controller IDs.
src/navigation/src/controller_type_publisher.cpp	Adds BT node that mutates blackboard `controller_type` based on mode + sequences.
src/navigation/include/controller_type_publisher.hpp	Declares the new BT node class.
src/navigation/CMakeLists.txt	Builds the new BT node into `navigation_nodes`.
src/bt_core/include/bt_engine.hpp	Includes the new node header for registration.
src/bt_core/src/bt_engine.cpp	Registers `ControllerTypePublisher` in the BT factory.
src/bt_core/bt/tree_node_model.xml	Adds node model entry/ports for `ControllerTypePublisher`.
src/bt_core/bt/black_blue_5.xml	Inserts `ControllerTypePublisher` into Take/GoHome flows; rearranges go_home DecisionCore placement.
src/bt_core/bt/white_yellow_4.xml	Adds a new plan XML using `ControllerTypePublisher`.
src/bt_core/params/mission_sequence_white.json	Modifies sequences (currently breaks JSON parsing).
src/bt_core/params/map_points_black.yaml	Adjusts staging distances for `YellowHome`.

Comments suppressed due to low confidence (1)

src/bt_core/bt/black_blue_5.xml:106

GoHomeSubTree now depends on target_idx/target_side/target_dir being produced outside the subtree (since DecisionCore was removed from the subtree). This coupling makes the subtree non-self-contained and easier to break if reused elsewhere. Consider keeping DecisionCore inside GoHomeSubTree (and placing ControllerTypePublisher before/after it) so the subtree can be executed independently.

    <BehaviorTree ID="GoHomeSubTree">
        <Sequence>
            <ControllerTypePublisher
                mode="Auto"
                controller_type="Slow"/>
            <OnDockAction
                targetPoseIdx="{target_idx}"
                targetPoseSideIdx="{target_side}"
                targetDirection="{target_dir}"
                dock_type="dock_y_loose_linearBoost"
                isPureDocking="false"
                finish_pose="{dock_result}"/>

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-04-05T11:30:27Z

src/navigation/src/controller_type_publisher.cpp

+    mode_ = input_mode;
+    blackboard_->set<std::string>("mode", mode_);
+
+    if (!blackboard_->get<std::string>("controller_type", current_controller_type)) {
+        current_controller_type = "Slow";
+        blackboard_->set<std::string>("controller_type", current_controller_type);
+    }
+
+    readBlackboard();
+
+    if (mode_ == "Auto" && pantry_info.data.empty() && collection_info.data.empty() &&
+        current_controller_type != controller_type_info_) {
+        blackboard_->set<std::string>("controller_type", controller_type_info_);
+        RCLCPP_INFO(node_->get_logger(),
+                    "[ControllerTypePublisher] set controller_type blackboard: %s -> %s (mode=%s)",
+                    current_controller_type.c_str(), controller_type_info_.c_str(), mode_.c_str());
+        return BT::NodeStatus::SUCCESS;
+    }
+
+    else if (mode_ == "Manual" && current_controller_type != controller_type_info_) {
+        blackboard_->set<std::string>("controller_type", controller_type_info_);
+        RCLCPP_INFO(node_->get_logger(),
+                    "[ControllerTypePublisher]  set controller_type blackboard: %s -> %s (mode=%s)",
+                    current_controller_type.c_str(), controller_type_info_.c_str(), mode_.c_str());
+        return BT::NodeStatus::SUCCESS;
+    }


The PR description says this node publishes to /controller_type, but the implementation only writes to the BT blackboard and never creates/publishes a ROS topic message. Either add a ROS publisher (e.g., std_msgs/msg/String) that actually updates /controller_type, or rename the node to reflect that it only sets blackboard state (and update the PR description accordingly).

Copilot · 2026-04-05T11:30:28Z

src/bt_core/params/mission_sequence_white.json

    "pantry_sequence": [
-        0,
-        1,
-        5,
-        4
+        0
+        // 1,
+        // 5,
+        // 4
    ],
    "collection_sequence": [
-        11,
-        10,
-        12,
-        13
+        11
+        // 10,
+        // 12,
+        // 13
    ]


mission_sequence_white.json is not valid JSON: the arrays now contain // comments and the first element (0 / 11) is missing a trailing comma. BTengine::addJsonPoint() will fail to parse this file and return early, leaving pantry_sequence/collection_sequence unset on the blackboard (which also affects ControllerTypePublisher Auto mode logic). Please revert to valid JSON (no comments, correct commas) or move the commented alternatives to a separate file.

Copilot · 2026-04-05T11:30:28Z

src/navigation/src/onDock.cpp

+    std::string controller_type;
+    if (!blackboard->get<std::string>("controller_type", controller_type) || controller_type.empty()) {
+        controller_type = "Normal";
+    }


Default controller_type handling is inconsistent across nodes: OnDockAction falls back to "Normal" when the blackboard key is missing/empty, but ControllerTypePublisher initializes missing values to "Slow". This can lead to different dock_type selection depending on which node runs first. Consider using a single shared default (and ideally constants/enums) for controller type values.

Copilot · 2026-04-05T11:30:28Z

src/navigation/src/onDock.cpp

    RCLCPP_INFO(node->get_logger(), "[OnDockAction] Initialized dock types: ny=%s, nx=%s, cy=%s, cx=%s", 
-                normal_dock_type_y_param.c_str(), normal_dock_type_x_param.c_str(),
-                cam_dock_type_y_param.c_str(), cam_dock_type_x_param.c_str());
+            normal_dock_type_y_param.c_str(), normal_dock_type_x_param.c_str(),
+            cam_dock_type_y_param.c_str(), cam_dock_type_x_param.c_str());


The initialization log only prints the normal/cam dock types. Since fast/slow dock type parameters were added, it will be hard to diagnose configuration issues without logging their resolved values too. Please extend this log (or add another) to include the fast/slow variants.

Copilot · 2026-04-05T11:30:28Z

src/navigation/src/controller_type_publisher.cpp

+    if (!getInput<std::string>("mode", input_mode)) {
+        input_mode = "Auto";
+    }
+
+    mode_ = input_mode;
+    blackboard_->set<std::string>("mode", mode_);
+
+    if (!blackboard_->get<std::string>("controller_type", current_controller_type)) {
+        current_controller_type = "Slow";
+        blackboard_->set<std::string>("controller_type", current_controller_type);
+    }
+
+    readBlackboard();
+
+    if (mode_ == "Auto" && pantry_info.data.empty() && collection_info.data.empty() &&
+        current_controller_type != controller_type_info_) {
+        blackboard_->set<std::string>("controller_type", controller_type_info_);
+        RCLCPP_INFO(node_->get_logger(),
+                    "[ControllerTypePublisher] set controller_type blackboard: %s -> %s (mode=%s)",
+                    current_controller_type.c_str(), controller_type_info_.c_str(), mode_.c_str());
+        return BT::NodeStatus::SUCCESS;
+    }
+
+    else if (mode_ == "Manual" && current_controller_type != controller_type_info_) {
+        blackboard_->set<std::string>("controller_type", controller_type_info_);
+        RCLCPP_INFO(node_->get_logger(),
+                    "[ControllerTypePublisher]  set controller_type blackboard: %s -> %s (mode=%s)",
+                    current_controller_type.c_str(), controller_type_info_.c_str(), mode_.c_str());
+        return BT::NodeStatus::SUCCESS;
+    }
+
+    RCLCPP_DEBUG(node_->get_logger(),
+                 "[ControllerTypePublisher] skipped update (mode=%s, collection=%zu, pantry=%zu, current=%s, target=%s)",
+                 mode_.c_str(), collection_info.data.size(), pantry_info.data.size(),
+                 current_controller_type.c_str(), controller_type_info_.c_str());
+
+    return BT::NodeStatus::SUCCESS;


mode_ is treated as a free-form string; if a typo is provided (e.g., "auto"), the node will silently skip updates and still return SUCCESS. Consider validating mode_ against the supported values (Auto/Manual) and either defaulting with a warning or returning FAILURE to surface the configuration error.

Copilot · 2026-04-05T11:30:28Z

src/bt_core/bt/black_blue_5.xml

+            <ControllerTypePublisher
+                mode="Auto"
+                controller_type="Slow"/>


In GoHomeSubTree, ControllerTypePublisher is configured with mode="Auto" while the PR description says GoHome should switch to Slow directly. With Auto mode it will only update when both pantry_sequence and collection_sequence are empty, so it may never switch to Slow during go_home. Use mode="Manual" here if the intent is an immediate change.

Copilot · 2026-04-05T11:30:29Z

src/bt_core/bt/black_blue_5.xml

+            <ControllerTypePublisher
+                mode="Auto"
+                controller_type="Fast"/>
            <!-- Dock to collection pdoint -->


Typo in XML comment: pdoint → point.

Suggested change

Copilot · 2026-04-05T11:30:29Z

src/bt_core/bt/white_yellow_4.xml

@@ -0,0 +1,182 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!-- ============================================= -->
+<!-- two robot battle: Black -->


File header comment says "two robot battle: Black" but this file is white_yellow_4.xml. This can be confusing when selecting/maintaining plans; please update the comment to match the actual robot/team/plan.

Suggested change

Copilot · 2026-04-05T11:30:29Z

src/bt_core/bt/white_yellow_4.xml

+    <!-- ============================================= -->
+    <BehaviorTree ID="TakeSubTree">
+        <Sequence>
+            <!-- Dock to collection pdoint -->


Typo in XML comment: pdoint → point.

Suggested change

Copilot · 2026-04-05T11:30:29Z

src/navigation/src/onDock.cpp

+    // Get dock_type from map_points and controller_type
    if (target_pose_idx < static_cast<int>(map_point_list.size())) {
        int dock_type_val = static_cast<int>(map_point_list[target_pose_idx].dock_type);
+        const bool is_fast = controller_type == "Fast";
+        const bool is_slow = controller_type == "Slow";
+
        switch (dock_type_val) {
            case 0: // MISSION_DOCK_Y
-                dock_type = normal_dock_type_y_param;
+                dock_type = is_fast ? fast_normal_dock_type_y_param : (is_slow ? slow_normal_dock_type_y_param : normal_dock_type_y_param);
                break;
            case 1: // MISSION_DOCK_X
-                dock_type = normal_dock_type_x_param;
+                dock_type = is_fast ? fast_normal_dock_type_x_param : (is_slow ? slow_normal_dock_type_x_param : normal_dock_type_x_param);
                break;
            case 2: // CAM_DOCK_Y
-                dock_type = cam_dock_type_y_param;
+                dock_type = is_fast ? fast_cam_dock_type_y_param : (is_slow ? slow_cam_dock_type_y_param : cam_dock_type_y_param);
                break;
            case 3: // CAM_DOCK_X
-                dock_type = cam_dock_type_x_param;
+                dock_type = is_fast ? fast_cam_dock_type_x_param : (is_slow ? slow_cam_dock_type_x_param : cam_dock_type_x_param);
                break;


OnDockAction still exposes an input port dock_type (and the BT XML passes dock_type=...), but setGoal() always overwrites dock_type from MapPointList (+ controller_type) and never reads the input port. This is confusing for BT authors. Either honor the dock_type input as an explicit override (with clear precedence) or remove the port/attributes from the model and trees to avoid a no-op parameter.

2006wu and others added 4 commits March 30, 2026 04:35

add controller_type_publisher

660ce06

0330 test controller_type_publisher

b61af9b

controller_type_publisher update

5d55261

manual mode update

e809f39

Copilot AI review requested due to automatic review settings April 5, 2026 11:25

Copilot AI reviewed Apr 5, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feat/change controller type#9

Feat/change controller type#9
2006wu wants to merge 4 commits intodevelopfrom
feat/change_controller_type

2006wu commented Apr 5, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Copilot AI Apr 5, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

	<!-- Dock to collection pdoint -->
	<!-- Dock to collection point -->

	<!-- two robot battle: Black -->
	<!-- two robot battle: white_yellow_4 -->

Conversation

2006wu commented Apr 5, 2026

Mode

Auto

Manual

How to use it ?

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 5, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants