AGVFM-320 Pallet pickup in odom space (#71)

* add GetPalletPositionFromDepthCameras.action and GetGoalFromPalletPosition.action

* set lift level action

* fix

* add missing method

* fixed naming for ports in action nodes GetGoalFromPalletPositionAction and GetPalletPositionFromDepthCamerasAction;

* add pallet_drive_obst and set LiftLevel inside pallet_pickup_without_obstacles BT

Co-authored-by: Johannes Plapp <johannes.plapp@logivations.com>
Co-authored-by: sofia.semianchuk <sofia.semianchuk@logivations.com>

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_adjust_pallet_goal_action_bt_node","nav2_narrow_pre_approach_action_bt_node","nav2_find_put_down_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_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_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_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_find_put_down_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_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_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

@@ -63,6 +63,15 @@ list(APPEND plugin_libs nav2_compute_pre_charging_goals_action_bt_node)
 add_library(nav2_get_charging_goal_from_marker_action_bt_node SHARED plugins/action/get_charging_goal_from_marker_action.cpp)
 list(APPEND plugin_libs nav2_get_charging_goal_from_marker_action_bt_node)
 
+add_library(nav2_get_pallet_position_from_depth_cameras_action_bt_node SHARED plugins/action/get_pallet_position_from_depth_cameras_action.cpp)
+list(APPEND plugin_libs nav2_get_pallet_position_from_depth_cameras_action_bt_node)
+
+add_library(nav2_get_goal_from_pallet_position_action_bt_node SHARED plugins/action/get_goal_from_pallet_position_action.cpp)
+list(APPEND plugin_libs nav2_get_goal_from_pallet_position_action_bt_node)
+
+add_library(nav2_set_lift_level_action_bt_node SHARED plugins/action/set_lift_level_action.cpp)
+list(APPEND plugin_libs nav2_set_lift_level_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)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_goal_from_pallet_position_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__GET_GOAL_FROM_PALLET_POSITION_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_GOAL_FROM_PALLET_POSITION_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/get_goal_from_pallet_position.hpp"
+#include "nav_msgs/msg/path.h"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class GetGoalFromPalletPositionAction : public BtActionNode<nav2_msgs::action::GetGoalFromPalletPosition>
+{
+public:
+  GetGoalFromPalletPositionAction(
+    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>("pallet_pickup_goal", "goal for picking up the pallet gets from the pallet position"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>("pallet_pos", "pallet position get from both agv fork depth cameras"),
+      });
+  }
+
+private:
+  bool first_time_{true};
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_GOAL_FROM_PALLET_POSITION_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pallet_position_from_depth_cameras_action.hpp

@@ -0,0 +1,53 @@
+// 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__GET_PALLET_POSITION_FROM_DEPTH_CAMERAS_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_PALLET_POSITION_FROM_DEPTH_CAMERAS_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/get_pallet_position_from_depth_cameras.hpp"
+#include "nav_msgs/msg/path.h"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class GetPalletPositionFromDepthCamerasAction : public BtActionNode<nav2_msgs::action::GetPalletPositionFromDepthCameras>
+{
+public:
+  GetPalletPositionFromDepthCamerasAction(
+    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>("pallet_pos","pallet position get from both agv fork depth cameras"),
+      });
+  }
+
+private:
+  bool first_time_{true};
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_PALLET_POSITION_FROM_DEPTH_CAMERAS_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/set_lift_level_action.hpp

@@ -0,0 +1,52 @@
+// 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__SET_LIFT_LEVEL_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SET_LIFT_LEVEL_ACTION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/set_lift_level.hpp"
+#include "nav2_behavior_tree/bt_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class SetLiftLevelAction : public BtActionNode<nav2_msgs::action::SetLiftLevel>
+{
+public:
+  SetLiftLevelAction(
+    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::InputPort<std::string>("lift_level", "Target lift level for the fork"),
+      });
+  }
+
+private:
+  bool first_time_{true};
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SET_LIFT_LEVEL_ACTION_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -57,6 +57,19 @@
       <output_port name="charging_goal_from_marker">charging_goal_from_marker</output_port>
     </Action>
 
+    <Action ID="GetPalletPositionFromDepthCameras">
+      <output_port name="pallet_pos">pallet position get from both agv fork depth cameras</output_port>
+    </Action>
+
+    <Action ID="GetGoalFromPalletPosition">
+      <input_port name="pallet_pos">goal for picking up the pallet gets from the pallet position</input_port>
+      <output_port name="pallet_pickup_goal">pallet position get from both agv fork depth cameras</output_port>
+    </Action>
+
+    <Action ID="SetLiftLevel">
+      <input_port name="lift_level">lift_level</input_port>
+    </Action>
+
     <Action ID="ComputeWaypointsToPose">
       <input_port name="goal">Destination to plan to</input_port>
       <output_port name="path">Path created by ComputeWaypointsToPose node</output_port>

nav2_behavior_tree/plugins/action/get_goal_from_pallet_position_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/get_goal_from_pallet_position_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GetGoalFromPalletPositionAction::GetGoalFromPalletPositionAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::GetGoalFromPalletPosition>(xml_tag_name, action_name, conf)
+{
+}
+
+void GetGoalFromPalletPositionAction::on_tick()
+{
+  getInput("pallet_pos", goal_.pallet_pos);
+}
+
+BT::NodeStatus GetGoalFromPalletPositionAction::on_success()
+{
+  setOutput("pallet_pickup_goal", result_.result->pallet_pickup_goal);
+  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::GetGoalFromPalletPositionAction>(
+        name, "get_goal_from_pallet_position", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::GetGoalFromPalletPositionAction>(
+    "GetGoalFromPalletPosition", builder);
+}

nav2_behavior_tree/plugins/action/get_pallet_position_from_depth_cameras_action.cpp

@@ -0,0 +1,55 @@
+// 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/get_pallet_position_from_depth_cameras_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+GetPalletPositionFromDepthCamerasAction::GetPalletPositionFromDepthCamerasAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::GetPalletPositionFromDepthCameras>(xml_tag_name, action_name, conf)
+{
+}
+
+void GetPalletPositionFromDepthCamerasAction::on_tick()
+{
+}
+
+BT::NodeStatus GetPalletPositionFromDepthCamerasAction::on_success()
+{
+  setOutput("pallet_pos", result_.result->pallet_pos);
+  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::GetPalletPositionFromDepthCamerasAction>(
+        name, "get_pallet_position_from_depth_cameras", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::GetPalletPositionFromDepthCamerasAction>(
+    "GetPalletPositionFromDepthCameras", builder);
+}

nav2_behavior_tree/plugins/action/set_lift_level_action.cpp

@@ -0,0 +1,55 @@
+// 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/set_lift_level_action.hpp"
+
+namespace nav2_behavior_tree
+{
+
+SetLiftLevelAction::SetLiftLevelAction(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtActionNode<nav2_msgs::action::SetLiftLevel>(xml_tag_name, action_name, conf)
+{
+}
+
+void SetLiftLevelAction::on_tick()
+{
+  getInput("lift_level", goal_.lift_level);
+}
+
+BT::NodeStatus SetLiftLevelAction::on_success()
+{
+  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::SetLiftLevelAction>(
+        name, "set_lift_level", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::SetLiftLevelAction>(
+    "SetLiftLevel", builder);
+}

nav2_bringup/bringup/params/forklift_params.yaml

@@ -254,6 +254,7 @@ controller_server:
         type: "line"
         line_end: [1.9, 0.0]
         line_start: [0.1, 0.0]
+      custom_obst_topic: "pallet_drive_obst"
       custom_narrow_obst_topic: "narrow_obst"
       min_obstacle_dist: 0.4
       include_dynamic_obstacles:  False
@@ -568,6 +569,9 @@ bt_navigator:
     - nav2_find_put_down_goal_action_bt_node
     - 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_compute_path_to_pose_action_bt_node
     - nav2_compute_path_to_pose_python_action_bt_node
     - nav2_compute_waypoints_to_pose_action_bt_node