AMRFM-1350: Decide whether pickup location is wrapping machine or standard pickup (ros-navigation#237)

* CheckBinType condition node

* For Brummer, only check whether bin_id = 2518_1 for roller_belt

Author: liyuanlogi

doc/parameters/param_list.md

@@ -8,7 +8,7 @@
 | enable_groot_monitoring | True | enable Groot live monitoring of the behavior tree |
 | groot_zmq_publisher_port | 1666 | change port of the zmq publisher needed for groot |
 | groot_zmq_server_port | 1667 | change port of the zmq server needed for groot |
-| plugin_lib_names | ["nav2_compute_pre_charging_goals_action_bt_node", "nav2_get_charging_goal_from_marker_action_bt_node", "nav2_get_pallet_position_from_depth_cameras_action_bt_node", "nav2_get_goal_from_pallet_position_action_bt_node", "nav2_set_lift_level_action_bt_node", "nav2_adjust_pallet_goal_action_bt_node","nav2_narrow_pre_approach_action_bt_node", "nav2_get_putdown_goal_action_bt_node", "nav2_check_zone_from_big_loaders_action_bt_node", "nav2_compute_path_to_pose_action_bt_node", "nav2_compute_waypoints_to_pose_action_bt_node", "nav2_compute_path_along_waypoints_action_bt_node", "nav2_find_free_goal_on_path_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_path_empty_condition_bt_node","nav2_is_lidar_triggered_condition_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_is_forklift_load_detected_condition_bt_node", "nav2_is_charging_action_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_call_service_bt_node", "nav2_set_node_parameters_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_mean_between_goals_condition_bt_node, nav2_move_goal_towards_other_goal_condition_bt_node"] | list of behavior tree node shared libraries |
+| plugin_lib_names | ["nav2_compute_pre_charging_goals_action_bt_node", "nav2_get_charging_goal_from_marker_action_bt_node", "nav2_get_pallet_position_from_depth_cameras_action_bt_node", "nav2_get_goal_from_pallet_position_action_bt_node", "nav2_set_lift_level_action_bt_node", "nav2_adjust_pallet_goal_action_bt_node","nav2_narrow_pre_approach_action_bt_node", "nav2_get_putdown_goal_action_bt_node", "nav2_check_zone_from_big_loaders_action_bt_node", "nav2_compute_path_to_pose_action_bt_node", "nav2_compute_waypoints_to_pose_action_bt_node", "nav2_compute_path_along_waypoints_action_bt_node", "nav2_find_free_goal_on_path_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_path_empty_condition_bt_node","nav2_is_lidar_triggered_condition_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_is_forklift_load_detected_condition_bt_node", "check_bin_type_condition_bt_node", "nav2_is_charging_action_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_call_service_bt_node", "nav2_set_node_parameters_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node", "nav2_mean_between_goals_condition_bt_node, nav2_move_goal_towards_other_goal_condition_bt_node"] | list of behavior tree node shared libraries |
 | transform_tolerance | 0.1 | TF transform tolerance |
 | global_frame | "map" | Reference frame |
 | robot_base_frame | "base_link" | Robot base frame |

nav2_behavior_tree/CMakeLists.txt

@@ -186,6 +186,9 @@ list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
 add_library(nav2_is_forklift_load_detected_condition_bt_node SHARED plugins/condition/is_forklift_load_detected.cpp)
 list(APPEND plugin_libs nav2_is_forklift_load_detected_condition_bt_node)
 
+add_library(nav2_check_bin_type_condition_bt_node SHARED plugins/condition/check_bin_type_condition.cpp)
+list(APPEND plugin_libs nav2_check_bin_type_condition_bt_node)
+
 add_library(nav2_is_charging_action_condition_bt_node SHARED plugins/condition/is_charging_action.cpp)
 list(APPEND plugin_libs nav2_is_charging_action_condition_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/check_bin_type_condition.hpp

@@ -0,0 +1,43 @@
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION_CHECK_BIN_TYPE_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION_CHECK_BIN_TYPE_CONDITION_HPP_
+
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include "behaviortree_cpp_v3/condition_node.h"
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree {
+
+class CheckBinTypeCondition : public BT::ConditionNode {
+public:
+  CheckBinTypeCondition(
+    const std::string &condition_name,
+    const BT::NodeConfiguration &conf);
+
+  CheckBinTypeCondition() = delete;
+
+  ~CheckBinTypeCondition();
+
+  BT::NodeStatus tick() override;
+
+  bool TargetBinTypeValid(std::string target_bin_type);
+  bool TargetBinTypeMatched(std::string target_bin_type, std::string bin_id);
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+    BT::InputPort<std::string>("target_bin_type", "AMR target bin type"),
+    BT::InputPort<std::string>("bin_id", "Bin ID to operate")
+    };
+  }
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  std::string target_bin_type;
+};
+
+} // namespace nav2_behavior_tree
+
+#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION_CHECK_BIN_TYPE_CONDITION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -263,6 +263,11 @@
       <input_port name="target_charging_action">target charging action</input_port>
     </Condition>
 
+    <Condition ID="CheckBinType">
+      <input_port name="target_bin_type">AMR target bin type</input_port>
+      <input_port name="bin_id">Bin ID to operate</input_port>
+    </Condition>
+
     <!-- ############################### CONTROL NODES ################################ -->
     <Control ID="PipelineSequence"/>
 

nav2_behavior_tree/plugins/condition/check_bin_type_condition.cpp

@@ -0,0 +1,64 @@
+#include <string>
+
+#include "nav2_behavior_tree/plugins/condition/check_bin_type_condition.hpp"
+
+namespace nav2_behavior_tree {
+
+CheckBinTypeCondition::CheckBinTypeCondition(
+  const std::string &condition_name,
+  const BT::NodeConfiguration &conf)
+: BT::ConditionNode(condition_name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  RCLCPP_DEBUG(node_->get_logger(), "Initialized a CheckBinTypeCondition BT node");
+}
+
+CheckBinTypeCondition::~CheckBinTypeCondition()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "Shutting down CheckBinTypeCondition BT node");
+}
+
+BT::NodeStatus CheckBinTypeCondition::tick() {
+  getInput("target_bin_type", target_bin_type);
+  std::string bin_id;
+  getInput<std::string>("bin_id", bin_id);
+
+  if (!TargetBinTypeValid(target_bin_type)){
+    RCLCPP_INFO(node_->get_logger(),
+                "target_bin_type:%s is not valid,"
+                "available target_bin_type should be either 'floor' or 'roller_belt'\n",target_bin_type);
+    return BT::NodeStatus::FAILURE;
+  }
+
+  if (TargetBinTypeMatched(target_bin_type, bin_id)){
+    RCLCPP_INFO(node_->get_logger(), "target_bin_type:%s matches input bin_id:%s\n", target_bin_type, bin_id);
+    return BT::NodeStatus::SUCCESS;
+  }
+  RCLCPP_INFO(node_->get_logger(), "target_bin_type:%s doesn't match input bin_id:%s\n", target_bin_type, bin_id);
+  return BT::NodeStatus::FAILURE;
+}
+
+bool CheckBinTypeCondition::TargetBinTypeValid(std::string target_bin_type)
+{
+  return target_bin_type == "floor" || target_bin_type == "roller_belt";
+}
+
+bool CheckBinTypeCondition::TargetBinTypeMatched(std::string target_bin_type, std::string bin_id)
+{
+  //  check whether bin_id = "2518_1" (Currently 2518_1 is the only roller_belt bin_id using in Brummer)
+  bool isRollerBelt = bin_id == "2518_1";
+  if (target_bin_type == "roller_belt"){
+    return isRollerBelt;
+  }
+  else {
+    return !isRollerBelt;
+  }
+}
+
+} // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<nav2_behavior_tree::CheckBinTypeCondition>(
+      "CheckBinType");
+}

nav2_bringup/bringup/params/forklift_params.yaml

@@ -495,6 +495,7 @@ bt_navigator:
       - nav2_is_charging_action_condition_bt_node
       - nav2_verify_bin_state_action_bt_node
       - nav2_barcode_scanning_success_condition_bt_node
+      - nav2_check_bin_type_condition_bt_node
 
 execute_goal:
   ros__parameters:

nav2_bt_navigator/src/bt_navigator.cpp

@@ -101,7 +101,8 @@ BtNavigator::BtNavigator()
     "nav2_is_charging_action_condition_bt_node",
     "nav2_mean_between_goals_condition_bt_node",
     "nav2_verify_bin_state_action_bt_node",
-    "nav2_barcode_scanning_success_condition_bt_node"
+    "nav2_barcode_scanning_success_condition_bt_node",
+    "nav2_check_bin_type_condition_bt_node"
   };
 
   declare_parameter("plugin_lib_names", plugin_libs);