diff --git a/nav2_segmentation/CMakeLists.txt b/nav2_segmentation/CMakeLists.txt new file mode 100644 index 00000000000..d9dd30df2bf --- /dev/null +++ b/nav2_segmentation/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.8) +project(nav2_segmentation) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +find_package(behaviortree_cpp REQUIRED) +find_package(nav2_behavior_tree REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_ros_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +nav2_package() + +rosidl_generate_interfaces(${PROJECT_NAME} + "action/SegmentImage.action" +) + +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + +add_library(nav2_segment_image_action_bt_node SHARED + src/segment_image_action.cpp +) +target_include_directories(nav2_segment_image_action_bt_node + PUBLIC + "$" + "$" +) +target_compile_definitions(nav2_segment_image_action_bt_node PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(nav2_segment_image_action_bt_node PUBLIC + behaviortree_cpp::behaviortree_cpp + nav2_behavior_tree::nav2_behavior_tree + nav2_ros_common::nav2_ros_common + rclcpp_action::rclcpp_action + ${cpp_typesupport_target} +) + +add_executable(nav2_segmentation_server + src/segmentation_server.cpp +) +target_link_libraries(nav2_segmentation_server + rclcpp::rclcpp + rclcpp_action::rclcpp_action + ${cpp_typesupport_target} +) + +install(TARGETS + nav2_segment_image_action_bt_node + nav2_segmentation_server + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(DIRECTORY behavior_trees + DESTINATION share/${PROJECT_NAME} +) + +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_libraries(nav2_segment_image_action_bt_node) +ament_export_dependencies( + behaviortree_cpp + nav2_behavior_tree + nav2_ros_common + rclcpp + rclcpp_action + rosidl_default_runtime +) +ament_export_targets(${PROJECT_NAME}) + +ament_package() diff --git a/nav2_segmentation/README.md b/nav2_segmentation/README.md new file mode 100644 index 00000000000..8feb977b888 --- /dev/null +++ b/nav2_segmentation/README.md @@ -0,0 +1,38 @@ +# nav2_segmentation + +`nav2_segmentation` is a bare-minimum discussion skeleton for connecting a Nav2 behavior tree node to a C++ ROS 2 action server. + +## What It Contains + +- A custom `SegmentImage` ROS 2 action +- A C++ Nav2 BT plugin named `SegmentImage` +- A C++ action server executable named `nav2_segmentation_server` +- A sample behavior tree XML + +## Behavior Tree Communication + +The behavior tree communicates with the package through the `SegmentImage` ROS 2 action. The BT plugin acts as the action client and the Python node hosts the action server on `segment_image` by default. +The behavior tree communicates with the package through the `SegmentImage` ROS 2 action. The BT plugin acts as the action client and the C++ node hosts the action server on `segment_image` by default. + +## Run + +```bash +ros2 run nav2_segmentation nav2_segmentation_server +``` + +Optional parameter: + +- `default_mask_topic`: result mask topic used by dummy response. + +## bt_navigator Plugin Configuration + +Add the shared library name to `plugin_lib_names`: + +```yaml +plugin_lib_names: + - nav2_segment_image_action_bt_node +``` + +## Notes + +This is dummy code only. There is no real SAM3 segmentation yet. diff --git a/nav2_segmentation/action/SegmentImage.action b/nav2_segmentation/action/SegmentImage.action new file mode 100644 index 00000000000..f5a0464f966 --- /dev/null +++ b/nav2_segmentation/action/SegmentImage.action @@ -0,0 +1,17 @@ +string image_topic +string text_prompt +float32 point_x +float32 point_y +int32 point_label +float32 box_min_x +float32 box_min_y +float32 box_max_x +float32 box_max_y +bool use_point_prompt +bool use_box_prompt +--- +bool success +string mask_topic +string message +--- +string current_state diff --git a/nav2_segmentation/behavior_trees/segment_image.xml b/nav2_segmentation/behavior_trees/segment_image.xml new file mode 100644 index 00000000000..73cf6e4ed07 --- /dev/null +++ b/nav2_segmentation/behavior_trees/segment_image.xml @@ -0,0 +1,21 @@ + + + + + + + diff --git a/nav2_segmentation/include/nav2_segmentation/segment_image_action.hpp b/nav2_segmentation/include/nav2_segmentation/segment_image_action.hpp new file mode 100644 index 00000000000..1537f07a2a9 --- /dev/null +++ b/nav2_segmentation/include/nav2_segmentation/segment_image_action.hpp @@ -0,0 +1,53 @@ +#ifndef NAV2_SEGMENTATION__SEGMENT_IMAGE_ACTION_HPP_ +#define NAV2_SEGMENTATION__SEGMENT_IMAGE_ACTION_HPP_ + +#include + +#include "nav2_behavior_tree/bt_action_node.hpp" +#include "nav2_segmentation/action/segment_image.hpp" + +namespace nav2_segmentation +{ + +class SegmentImageAction + : public nav2_behavior_tree::BtActionNode +{ + using Action = nav2_segmentation::action::SegmentImage; + +public: + SegmentImageAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + static BT::PortsList providedPorts() + { + return providedBasicPorts( + { + BT::InputPort("image_topic", "/camera/image", "Input image topic"), + BT::InputPort("text_prompt", "", "Text prompt"), + BT::InputPort("point_x", 0.0F, "Point prompt x coordinate"), + BT::InputPort("point_y", 0.0F, "Point prompt y coordinate"), + BT::InputPort("point_label", 1, "Point label (1 foreground, 0 background)"), + BT::InputPort("box_min_x", 0.0F, "Box minimum x"), + BT::InputPort("box_min_y", 0.0F, "Box minimum y"), + BT::InputPort("box_max_x", 0.0F, "Box maximum x"), + BT::InputPort("box_max_y", 0.0F, "Box maximum y"), + BT::InputPort("use_point_prompt", false, "Use point prompt"), + BT::InputPort("use_box_prompt", false, "Use box prompt"), + BT::OutputPort("success", "Segmentation action success"), + BT::OutputPort("mask_topic", "Output mask topic"), + BT::OutputPort("message", "Segmentation status message") + }); + } + + void on_tick() override; + BT::NodeStatus on_success() override; + BT::NodeStatus on_aborted() override; + BT::NodeStatus on_cancelled() override; + void on_timeout() override; +}; + +} // namespace nav2_segmentation + +#endif // NAV2_SEGMENTATION__SEGMENT_IMAGE_ACTION_HPP_ diff --git a/nav2_segmentation/package.xml b/nav2_segmentation/package.xml new file mode 100644 index 00000000000..9de8de4cef0 --- /dev/null +++ b/nav2_segmentation/package.xml @@ -0,0 +1,28 @@ + + + + nav2_segmentation + 0.0.1 + Bare-minimum Nav2 segmentation discussion skeleton with BT action integration. + User + Apache-2.0 + + ament_cmake + + nav2_common + + backward_ros + behaviortree_cpp + nav2_behavior_tree + nav2_ros_common + rclcpp + rclcpp_action + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/nav2_segmentation/src/segment_image_action.cpp b/nav2_segmentation/src/segment_image_action.cpp new file mode 100644 index 00000000000..b765766fc9b --- /dev/null +++ b/nav2_segmentation/src/segment_image_action.cpp @@ -0,0 +1,76 @@ +#include +#include + +#include "nav2_segmentation/segment_image_action.hpp" +#include "behaviortree_cpp/bt_factory.h" + +namespace nav2_segmentation +{ + +SegmentImageAction::SegmentImageAction( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: BtActionNode(xml_tag_name, action_name, conf) +{ +} + +void SegmentImageAction::on_tick() +{ + getInput("image_topic", goal_.image_topic); + getInput("text_prompt", goal_.text_prompt); + getInput("point_x", goal_.point_x); + getInput("point_y", goal_.point_y); + getInput("point_label", goal_.point_label); + getInput("box_min_x", goal_.box_min_x); + getInput("box_min_y", goal_.box_min_y); + getInput("box_max_x", goal_.box_max_x); + getInput("box_max_y", goal_.box_max_y); + getInput("use_point_prompt", goal_.use_point_prompt); + getInput("use_box_prompt", goal_.use_box_prompt); +} + +BT::NodeStatus SegmentImageAction::on_success() +{ + setOutput("success", result_.result->success); + setOutput("mask_topic", result_.result->mask_topic); + setOutput("message", result_.result->message); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus SegmentImageAction::on_aborted() +{ + setOutput("success", false); + setOutput("mask_topic", result_.result->mask_topic); + setOutput("message", result_.result->message); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus SegmentImageAction::on_cancelled() +{ + setOutput("success", false); + setOutput("mask_topic", ""); + setOutput("message", "Segmentation action cancelled."); + return BT::NodeStatus::SUCCESS; +} + +void SegmentImageAction::on_timeout() +{ + setOutput("success", false); + setOutput("mask_topic", ""); + setOutput("message", "Segmentation action timed out waiting for server response."); +} + +} // namespace nav2_segmentation + +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = + [](const std::string & name, const BT::NodeConfiguration & config) + { + return std::make_unique( + name, "segment_image", config); + }; + + factory.registerBuilder("SegmentImage", builder); +} diff --git a/nav2_segmentation/src/segmentation_server.cpp b/nav2_segmentation/src/segmentation_server.cpp new file mode 100644 index 00000000000..7c2a44d68b4 --- /dev/null +++ b/nav2_segmentation/src/segmentation_server.cpp @@ -0,0 +1,76 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "nav2_segmentation/action/segment_image.hpp" + +namespace nav2_segmentation +{ + +class SegmentationServer : public rclcpp::Node +{ +public: + using SegmentImage = nav2_segmentation::action::SegmentImage; + using GoalHandleSegmentImage = rclcpp_action::ServerGoalHandle; + + SegmentationServer() + : Node("nav2_segmentation") + { + declare_parameter("action_name", "segment_image"); + declare_parameter("default_mask_topic", "/segmentation/mask"); + + const auto action_name = get_parameter("action_name").as_string(); + default_mask_topic_ = get_parameter("default_mask_topic").as_string(); + + action_server_ = rclcpp_action::create_server( + this, + action_name, + std::bind(&SegmentationServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&SegmentationServer::handle_cancel, this, std::placeholders::_1), + std::bind(&SegmentationServer::handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "Started dummy C++ segmentation action server on %s", action_name.c_str()); + } + +private: + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, std::shared_ptr goal) + { + (void)goal; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr) + { + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + auto feedback = std::make_shared(); + feedback->current_state = "dummy_complete"; + goal_handle->publish_feedback(feedback); + + auto result = std::make_shared(); + result->success = true; + result->mask_topic = default_mask_topic_; + result->message = "Dummy C++ segmentation result for discussion."; + goal_handle->succeed(result); + } + + std::string default_mask_topic_; + rclcpp_action::Server::SharedPtr action_server_; +}; + +} // namespace nav2_segmentation + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}