WMO-56730 Adjust pallet goal action and bt node (#19)

* create action and node

* add behavior tree

Author: jplapp

doc/parameters/param_list.md

@@ -5,7 +5,7 @@
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | default_bt_xml_filename | N/A | path to the default behavior tree XML description |
-| plugin_lib_names | ["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_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_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"] | list of behavior tree node shared libraries |
+| plugin_lib_names | ["nav2_adjust_pallet_goal_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_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_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"] | 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

@@ -55,6 +55,9 @@ ament_target_dependencies(${library_name}
 add_library(nav2_compute_path_to_pose_action_bt_node SHARED plugins/action/compute_path_to_pose_action.cpp)
 list(APPEND plugin_libs nav2_compute_path_to_pose_action_bt_node)
 
+add_library(nav2_adjust_pallet_goal_action_bt_node SHARED plugins/action/adjust_pallet_goal_action.cpp)
+list(APPEND plugin_libs nav2_adjust_pallet_goal_action_bt_node)
+
 add_library(nav2_compute_path_to_pose_python_action_bt_node SHARED plugins/action/compute_path_to_pose_python_action.cpp)
 list(APPEND plugin_libs nav2_compute_path_to_pose_python_action_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/adjust_pallet_goal_action.hpp

@@ -0,0 +1,54 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ADJUST_PALLET_GOAL_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ADJUST_PALLET_GOAL_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/adjust_pallet_goal.hpp"
+#include "nav_msgs/msg/path.h"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class AdjustPalletGoalAction : public BtActionNode<nav2_msgs::action::AdjustPalletGoal>
+{
+public:
+  AdjustPalletGoalAction(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+
+  BT::NodeStatus on_success() override;
+  
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts(
+      {
+        BT::OutputPort<geometry_msgs::msg::PoseStamped>("goal", "Adjusted goal"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Input goal that should be adjusted"),
+      });
+  }
+  
+private:
+  bool first_time_{true};
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__ADJUST_PALLET_GOAL_ACTION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -28,7 +28,10 @@
       <input_port name="planner_id"/>
     </Action>
 
-
+    <Action ID="AdjustPalletGoal">
+      <input_port name="input_goal">Input goal that should be adjusted</input_port>
+      <output_port name="goal">Adjusted goal</output_port>
+    </Action>
 
     <Action ID="ComputeWaypointsToPose">
       <input_port name="goal">Destination to plan to</input_port>

nav2_behavior_tree/plugins/action/adjust_pallet_goal_action.cpp

@@ -0,0 +1,56 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// 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 <memory>
+#include <string>
+
+#include "nav2_behavior_tree/plugins/action/adjust_pallet_goal_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+AdjustPalletGoalAction::AdjustPalletGoalAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::AdjustPalletGoal>(xml_tag_name, action_name, conf)
+{
+}
+
+void AdjustPalletGoalAction::on_tick()
+{
+  getInput("input_goal", goal_.input_goal);
+}
+
+BT::NodeStatus AdjustPalletGoalAction::on_success()
+{
+  setOutput("goal", result_.result->pose);
+  return BT::NodeStatus::SUCCESS;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::AdjustPalletGoalAction>(
+        name, "adjust_pallet_goal", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::AdjustPalletGoalAction>(
+    "AdjustPalletGoal", builder);
+}

nav2_bringup/bringup/params/forklift_params.yaml

@@ -127,6 +127,7 @@ bt_navigator:
     odom_topic: /odom
     default_bt_xml_filename: "/code/ros2_ws/src/navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_v1.xml"
     plugin_lib_names:
+    - nav2_adjust_pallet_goal_action_bt_node
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_to_pose_python_action_bt_node
     - nav2_compute_waypoints_to_pose_action_bt_node

nav2_bt_navigator/behavior_trees/drive_between_pallets.xml

@@ -0,0 +1,17 @@
+<!--
+  This Behavior Tree drives between pallets, and continously searches for the final destination
+-->
+
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <PipelineSequence name="DriveBetweenPallets">
+      <RateController hz="1.0">
+        <Sequence name="AdjustAndReplan">
+          <AdjustPalletGoal input_goal="{goal}" goal="{adjusted_goal}"/>
+          <ComputePathToPosePython goal="{adjusted_goal}" path="{path}" planner_id="GridBased"/>
+        </Sequence>          
+      </RateController>
+      <FollowPath path="{path}" controller_id="DynamicFollowPath"/>
+    </PipelineSequence>
+  </BehaviorTree>
+</root>

nav2_bt_navigator/src/bt_navigator.cpp

@@ -37,6 +37,7 @@ BtNavigator::BtNavigator()
   RCLCPP_INFO(get_logger(), "Creating");
 
   const std::vector<std::string> plugin_libs = {
+    "nav2_adjust_pallet_goal_action_bt_node",
     "nav2_compute_path_to_pose_action_bt_node",
     "nav2_compute_path_to_pose_python_action_bt_node",
     "nav2_compute_waypoints_to_pose_action_bt_node",

nav2_msgs/CMakeLists.txt

@@ -29,6 +29,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SaveMap.srv"
   "action/BackUp.action"
   "action/ComputePathToPose.action"
+  "action/AdjustPalletGoal.action"
   "action/ComputeWaypointsToPose.action"
   "action/ComputePathAlongWaypoints.action"
   "action/FindFreeGoalOnPath.action"

nav2_msgs/action/AdjustPalletGoal.action

@@ -0,0 +1,7 @@
+#goal definition
+geometry_msgs/PoseStamped input_goal
+---
+#result definition
+geometry_msgs/PoseStamped pose
+---
+#feedback