first commit for goaroundwaypoints

robotx_behavior_tree/include/robotx_behavior_tree/action_node.hpp

@@ -190,6 +190,12 @@ class ActionROS2Node : public BT::StatefulActionNode, public rclcpp::Node
       });
   }
 
+  enum class TurnDirection
+  {
+    Right, 
+    Left
+  }; 
+
   std::optional<geometry_msgs::msg::Point> getPoint(
     const robotx_behavior_msgs::msg::TaskObject & task_object) const
   {
@@ -307,6 +313,79 @@ 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;
+  }
+
+  std::vector<geometry_msgs::msg::Pose> getGoaroundWaypoints(
+    const robotx_behavior_msgs::msg::TaskObject & obj, 
+    double radius, 
+    TurnDirection direction,
+    size_t num_split = 4) const
+  {
+    // std::vector<geometry_msgs::msg::Pose> waypoints;
+    // // Get current pose(Quaternion)
+    // // std::optional<geometry_msgs::msg::PoseStamped::SharedPtr> current_pose =  getCurrentPose();
+    // const auto current_pose = getCurrentPose();
+    // if(!current_pose){
+    //   return {};
+    // }
+
+    // // 
+    // // Convert Quaternion to Euler
+    // // To 
+    // const auto ship_yaw = quaternion_operation::convertQuaternionToEulerAngle(current_pose.value()->pose.orientation).z  ;
+
+    // // const auto ship_yaw = quaternion_operation::convertQuaternionToEulerAngle(current_pose.orientation);
+
+    // // double in_sqrt = math::sqrt(pow())
+    // // double waypoint_x = ()
+    // // 頑張って計算
+    // // 分割あたりの角度を計算
+    // // int per_rad = (2 * std::) / num_split; 
+
+    // // for(int cnt = 1; cnt <= per_rad; cnt++)
+    // // {
+    // //   // TODO: 配列に置換
+    // //   double x = radius * cos(per_rad * cnt) + obj.x;
+    // //   double y = radius * sin(per_rad * cnt) + obj.y;
+    // // }
+    // switch (direction)
+    // {
+    // case TurnDirection::Right:
+
+    //   break;
+
+    // case TurnDirection::Left:
+    //   break;
+    // }
+  }
+
+
 };
 }  // namespace robotx_behavior_tree
 

robotx_behavior_tree/plugins/action/go_around_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_bt_planner/behavior_trees/go_around_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/go_around_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/go_around_object.xml