diff --git a/robotx_behavior_msgs/CMakeLists.txt b/robotx_behavior_msgs/CMakeLists.txt index 2996853..77f42a2 100644 --- a/robotx_behavior_msgs/CMakeLists.txt +++ b/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() diff --git a/robotx_behavior_tree/CMakeLists.txt b/robotx_behavior_tree/CMakeLists.txt index 93ba759..cc53273 100644 --- a/robotx_behavior_tree/CMakeLists.txt +++ b/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() diff --git a/robotx_behavior_tree/include/robotx_behavior_tree/action_node.hpp b/robotx_behavior_tree/include/robotx_behavior_tree/action_node.hpp index f115bef..5d00af3 100644 --- a/robotx_behavior_tree/include/robotx_behavior_tree/action_node.hpp +++ b/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 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 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 diff --git a/robotx_behavior_tree/plugins/action/move_to_front_pose_of_object.cpp b/robotx_behavior_tree/plugins/action/move_to_front_pose_of_object.cpp new file mode 100644 index 0000000..b6d7fa2 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#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("/move_base_simple/goal", 1); + } + static BT::PortsList providedPorts() + { + return appendPorts( + ActionROS2Node::providedPorts(), {BT::InputPort("object_type")}); + } + +private: + rclcpp::TimerBase::SharedPtr update_position_timer_; + float distance_; + double goal_tolerance_; + geometry_msgs::msg::PoseStamped goal_; + rclcpp::Publisher::SharedPtr goal_pub_front_pose_of_object_; + std::vector 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("object_type"); + + if (task_objects_array) { + if (object_type.value() == "red_bouy") { + target_objects_array_ = + filter(task_objects_array.value(), static_cast(Buoy::BUOY_RED)); + } else if (object_type.value() == "green_bouy") { + target_objects_array_ = + filter(task_objects_array.value(), static_cast(Buoy::BUOY_GREEN)); + } else if (object_type.value() == "white_bouy") { + target_objects_array_ = + filter(task_objects_array.value(), static_cast(Buoy::BUOY_WHITE)); + } else if (object_type.value() == "black_bouy") { + target_objects_array_ = + filter(task_objects_array.value(), static_cast(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(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) diff --git a/robotx_behavior_tree/test/src/test_action_node.cpp b/robotx_behavior_tree/test/src/test_action_node.cpp new file mode 100644 index 0000000..067edd7 --- /dev/null +++ b/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 hkt8g2r6kin@gmail.com + * @brief test code for Action Node + * @version 0.1 + * @date 2023-10-17 + * + * @copyright Copyright (c) 2023 + * + */ +#include + +#include + +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(); +} \ No newline at end of file diff --git a/robotx_bt_planner/behavior_trees/move_to_front_pose_of_object.xml b/robotx_bt_planner/behavior_trees/move_to_front_pose_of_object.xml new file mode 100644 index 0000000..e59f739 --- /dev/null +++ b/robotx_bt_planner/behavior_trees/move_to_front_pose_of_object.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/robotx_bt_planner/config/move_to_front_pose_of_object.yaml b/robotx_bt_planner/config/move_to_front_pose_of_object.yaml new file mode 100644 index 0000000..67ee120 --- /dev/null +++ b/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 \ No newline at end of file