Merge pull request #94 from OUXT-Polaris/feature/go_object_waypoint

オブジェクトの前に移動する関数を追加

robotx_behavior_msgs/CMakeLists.txt

@@ -31,8 +31,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
-find_package(ament_lint_auto REQUIRED)
-ament_lint_auto_find_test_dependencies()
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
 endif()
 
 ament_package()

robotx_behavior_tree/CMakeLists.txt

@@ -14,9 +14,7 @@ endif()
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
-#include_directories(include ${CMAKE_CURRENT_BINARY_DIR})
-
-#set(library_name ${PROJECT_NAME})
+include_directories(include ${EIGEN3_INCLUDE_DIR})
 
 ament_auto_add_library(example_action SHARED plugins/action/example_action.cpp)
 target_compile_definitions(example_action PRIVATE BT_PLUGIN_EXPORT)
@@ -36,6 +34,9 @@ target_compile_definitions(set_turn_action PRIVATE BT_PLUGIN_EXPORT)
 ament_auto_add_library(move_to_gate_action SHARED plugins/action/move_to_gate_action.cpp)
 target_compile_definitions(move_to_gate_action PRIVATE BT_PLUGIN_EXPORT)
 
+ament_auto_add_library(move_to_front_pose_of_object SHARED plugins/action/move_to_front_pose_of_object.cpp)
+target_compile_definitions(move_to_front_pose_of_object PRIVATE BT_PLUGIN_EXPORT)
+
 ament_auto_add_library(to_marker SHARED src/to_marker.cpp)
 
 install(TARGETS
@@ -50,6 +51,8 @@ install(DIRECTORY models DESTINATION share/${PROJECT_NAME})
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_auto_add_gtest(test_action_node test/src/test_action_node.cpp)
 endif()
 
 ament_auto_package()

robotx_behavior_tree/include/robotx_behavior_tree/action_node.hpp

@@ -139,7 +139,6 @@ class ActionROS2Node : public BT::StatefulActionNode, public rclcpp::Node
   DEFINE_GET_INPUT(
     TaskObjects, robotx_behavior_msgs::msg::TaskObjectsArrayStamped::SharedPtr, "task_objects");
 #undef DEFINE_GET_INPUT
-
   template <typename T1, typename T2>
   double getDistance(const T1 & p1, const T2 & p2) const
   {
@@ -202,7 +201,6 @@ class ActionROS2Node : public BT::StatefulActionNode, public rclcpp::Node
     p.z = task_object.z[0];
     return p;
   }
-
   geometry_msgs::msg::Point2D getPoint2D(
     const robotx_behavior_msgs::msg::TaskObject & task_object) const
   {
@@ -307,6 +305,33 @@ class ActionROS2Node : public BT::StatefulActionNode, public rclcpp::Node
     p.orientation = quaternion_operation::convertEulerAngleToQuaternion(goal_rpy);
     return p;
   }
+
+  std::optional<geometry_msgs::msg::Pose> getFrontPoseOfObject(
+    const robotx_behavior_msgs::msg::TaskObject & obj, const double distance = 7.0) const
+  {
+    const auto current_pose = getCurrentPose();
+    if (!current_pose) {
+      return std::nullopt;
+    }
+    double delta_x = obj.x - current_pose.value()->pose.position.x;
+    double delta_y = obj.y - current_pose.value()->pose.position.y;
+    const double minimum_delta = 0.1;
+    if (abs(delta_x) < minimum_delta) {
+      delta_x = minimum_delta;
+    }
+    if (abs(delta_y) < minimum_delta) {
+      delta_y = minimum_delta;
+    }
+    double theta = std::atan2(delta_y, delta_x);
+    geometry_msgs::msg::Pose p;
+    p.position.x = obj.x - distance * std::cos(theta);
+    p.position.y = obj.y - distance * std::sin(theta);
+    p.position.z = 0.0;
+    geometry_msgs::msg::Vector3 goal_rpy;
+    goal_rpy.z = theta;
+    p.orientation = quaternion_operation::convertEulerAngleToQuaternion(goal_rpy);
+    return p;
+  }
 };
 }  // namespace robotx_behavior_tree
 

robotx_behavior_tree/plugins/action/move_to_front_pose_of_object.cpp

@@ -0,0 +1,141 @@
+// Copyright (c) 2024, OUXT-Polaris
+//
+// 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 <algorithm>
+#include <iostream>
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "hermite_path_msgs/msg/planner_status.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "robotx_behavior_msgs/msg/task_object.hpp"
+#include "robotx_behavior_tree/action_node.hpp"
+
+namespace robotx_behavior_tree
+{
+class MoveToFrontPoseOfObject : public ActionROS2Node
+{
+public:
+  MoveToFrontPoseOfObject(const std::string & name, const BT::NodeConfiguration & config)
+  : ActionROS2Node(name, config)
+  {
+    declare_parameter("goal_tolerance", 1.0);
+    get_parameter("goal_tolerance", goal_tolerance_);
+    goal_pub_front_pose_of_object_ =
+      this->create_publisher<geometry_msgs::msg::PoseStamped>("/move_base_simple/goal", 1);
+  }
+  static BT::PortsList providedPorts()
+  {
+    return appendPorts(
+      ActionROS2Node::providedPorts(), {BT::InputPort<std::string>("object_type")});
+  }
+
+private:
+  rclcpp::TimerBase::SharedPtr update_position_timer_;
+  float distance_;
+  double goal_tolerance_;
+  geometry_msgs::msg::PoseStamped goal_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pub_front_pose_of_object_;
+  std::vector<robotx_behavior_msgs::msg::TaskObject> target_objects_array_;
+
+  enum class Buoy : short {
+    BUOY_RED = robotx_behavior_msgs::msg::TaskObject::BUOY_RED,
+    BUOY_GREEN = robotx_behavior_msgs::msg::TaskObject::BUOY_GREEN,
+    BUOY_WHITE = robotx_behavior_msgs::msg::TaskObject::BUOY_WHITE,
+    BUOY_BLACK = robotx_behavior_msgs::msg::TaskObject::BUOY_BLACK
+  };
+  enum class Status : short {
+    WAITING_FOR_GOAL = hermite_path_msgs::msg::PlannerStatus::WAITING_FOR_GOAL,
+    MOVING_TO_GOAL = hermite_path_msgs::msg::PlannerStatus::MOVING_TO_GOAL,
+    AVOIDING = hermite_path_msgs::msg::PlannerStatus::MOVING_TO_GOAL
+  };
+
+protected:
+  BT::NodeStatus onStart() override
+  {
+    const auto status_planner = getPlannerStatus();
+    const auto task_objects_array = getTaskObjects();
+    if (task_objects_array) {
+      RCLCPP_INFO(get_logger(), "get task_objects");
+      return BT::NodeStatus::RUNNING;
+    } else {
+      return BT::NodeStatus::FAILURE;
+    }
+  }
+
+  BT::NodeStatus onRunning() override
+  {
+    const auto status_planner = getPlannerStatus();
+    const auto pose = getCurrentPose();
+    const auto task_objects_array = getTaskObjects();
+
+    auto object_type = this->getInput<std::string>("object_type");
+
+    if (task_objects_array) {
+      if (object_type.value() == "red_bouy") {
+        target_objects_array_ =
+          filter(task_objects_array.value(), static_cast<short>(Buoy::BUOY_RED));
+      } else if (object_type.value() == "green_bouy") {
+        target_objects_array_ =
+          filter(task_objects_array.value(), static_cast<short>(Buoy::BUOY_GREEN));
+      } else if (object_type.value() == "white_bouy") {
+        target_objects_array_ =
+          filter(task_objects_array.value(), static_cast<short>(Buoy::BUOY_WHITE));
+      } else if (object_type.value() == "black_bouy") {
+        target_objects_array_ =
+          filter(task_objects_array.value(), static_cast<short>(Buoy::BUOY_BLACK));
+      } else {
+        return BT::NodeStatus::FAILURE;
+      }
+    }
+
+    sortBy2DDistance(target_objects_array_, pose.value()->pose.position);
+    if (target_objects_array_.empty()) {
+      return BT::NodeStatus::FAILURE;
+    }
+
+    const auto front_pose = getFrontPoseOfObject(target_objects_array_[0], 7.0);
+    get_parameter("goal_tolerance", goal_tolerance_);
+    goal_.header.frame_id = "map";
+    if (front_pose) {
+      goal_.pose.position.x = front_pose.value().position.x;
+      goal_.pose.position.y = front_pose.value().position.y;
+      goal_.pose.position.z = front_pose.value().position.z;
+      goal_.pose.orientation.w = front_pose.value().orientation.w;
+      goal_.pose.orientation.x = front_pose.value().orientation.x;
+      goal_.pose.orientation.y = front_pose.value().orientation.y;
+      goal_.pose.orientation.z = front_pose.value().orientation.z;
+    }
+    goal_.header.stamp = get_clock()->now();
+    if (status_planner.value()->status == static_cast<short>(Status::WAITING_FOR_GOAL)) {
+      goal_pub_front_pose_of_object_->publish(goal_);
+    }
+    distance_ = getDistance(pose.value()->pose.position, goal_.pose.position);
+
+    if (distance_ < goal_tolerance_) {
+      RCLCPP_INFO(get_logger(), "Throgh Goal : SUCCESS");
+      return BT::NodeStatus::SUCCESS;
+    }
+
+    return BT::NodeStatus::RUNNING;
+  }
+};
+}  // namespace robotx_behavior_tree
+
+#include "behavior_tree_action_builder/register_nodes.hpp"  // NOLINT
+
+REGISTER_NODES(robotx_behavior_tree, MoveToFrontPoseOfObject)

robotx_behavior_tree/test/src/test_action_node.cpp

@@ -0,0 +1,56 @@
+// Copyright (c) 2023 OUXT Polaris
+//
+// 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.
+
+/**
+ * @file test_action_node.cpp
+ * @author Kento Hirogaki [email protected]
+ * @brief test code for Action Node
+ * @version 0.1
+ * @date 2023-10-17
+ *
+ * @copyright Copyright (c) 2023
+ *
+ */
+#include <gtest/gtest.h>
+
+#include <robotx_behavior_tree/action_node.hpp>
+
+TEST(TestSuite, testCase1)
+{
+  robotx_behavior_msgs::msg::TaskObject object;
+  // robotx_behavior_tree::ActionROS2Node *testtest();
+  object.x = 0;
+  object.y = 0;
+
+  // object->x = 0;
+  // object->y = 0;
+  // robotx_behavior_tree::ActionROS2Node::getFrontPoseForWaypoint(object, 2.0);
+
+  // robotx_behavior_tree::getFrontPoseForWaypoint(object, 2.0);
+  // robotx_behavior_tree::ActionROS2Node::getPoint(object);
+  EXPECT_EQ(true, true);
+}
+
+/**
+ * @brief Run all the tests that were declared with TEST()
+ *
+ * @param argc
+ * @param argv
+ * @return int
+ */
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

robotx_bt_planner/behavior_trees/move_to_front_pose_of_object.xml

@@ -0,0 +1,7 @@
+<root main_tree_to_execute = "MainTree" >
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <Action ID="MoveToFrontPoseOfObject" task_objects="{task_objects}" object_type="red_bouy"/>
+        </Sequence>
+    </BehaviorTree>
+</root>

robotx_bt_planner/config/move_to_front_pose_of_object.yaml

@@ -0,0 +1,9 @@
+plugins:
+  - package: robotx_behavior_tree
+    name:
+      - move_to_front_pose_of_object
+      - move_goal_action
+behavior:
+  description:
+    package: robotx_bt_planner
+    path : behavior_trees/move_to_front_pose_of_object.xml