Nav2 Segmentation Node

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
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+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()

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.

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

nav2_segmentation/behavior_trees/segment_image.xml

@@ -0,0 +1,21 @@
+<root BTCPP_format="4">
+  <BehaviorTree ID="SegmentImageTree">
+    <Sequence name="segment_image_sequence">
+      <SegmentImage
+        image_topic="/camera/image"
+        text_prompt="target"
+        use_point_prompt="true"
+        point_x="320.0"
+        point_y="240.0"
+        point_label="1"
+        use_box_prompt="true"
+        box_min_x="200.0"
+        box_min_y="120.0"
+        box_max_x="440.0"
+        box_max_y="360.0"
+        mask_topic="{mask_topic}"
+        success="{segment_success}"
+        message="{segment_message}"/>
+    </Sequence>
+  </BehaviorTree>
+</root>

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 <string>
+
+#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<nav2_segmentation::action::SegmentImage>
+{
+  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<std::string>("image_topic", "/camera/image", "Input image topic"),
+        BT::InputPort<std::string>("text_prompt", "", "Text prompt"),
+        BT::InputPort<float>("point_x", 0.0F, "Point prompt x coordinate"),
+        BT::InputPort<float>("point_y", 0.0F, "Point prompt y coordinate"),
+        BT::InputPort<int>("point_label", 1, "Point label (1 foreground, 0 background)"),
+        BT::InputPort<float>("box_min_x", 0.0F, "Box minimum x"),
+        BT::InputPort<float>("box_min_y", 0.0F, "Box minimum y"),
+        BT::InputPort<float>("box_max_x", 0.0F, "Box maximum x"),
+        BT::InputPort<float>("box_max_y", 0.0F, "Box maximum y"),
+        BT::InputPort<bool>("use_point_prompt", false, "Use point prompt"),
+        BT::InputPort<bool>("use_box_prompt", false, "Use box prompt"),
+        BT::OutputPort<bool>("success", "Segmentation action success"),
+        BT::OutputPort<std::string>("mask_topic", "Output mask topic"),
+        BT::OutputPort<std::string>("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_

nav2_segmentation/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_segmentation</name>
+  <version>0.0.1</version>
+  <description>Bare-minimum Nav2 segmentation discussion skeleton with BT action integration.</description>
+  <maintainer email="user@example.com">User</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>nav2_common</build_depend>
+
+  <depend>backward_ros</depend>
+  <depend>behaviortree_cpp</depend>
+  <depend>nav2_behavior_tree</depend>
+  <depend>nav2_ros_common</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rosidl_default_generators</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

nav2_segmentation/src/segment_image_action.cpp

@@ -0,0 +1,76 @@
+#include <memory>
+#include <string>
+
+#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<Action>(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<nav2_segmentation::SegmentImageAction>(
+        name, "segment_image", config);
+    };
+
+  factory.registerBuilder<nav2_segmentation::SegmentImageAction>("SegmentImage", builder);
+}

nav2_segmentation/src/segmentation_server.cpp

@@ -0,0 +1,76 @@
+#include <memory>
+#include <string>
+
+#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<SegmentImage>;
+
+  SegmentationServer()
+  : Node("nav2_segmentation")
+  {
+    declare_parameter<std::string>("action_name", "segment_image");
+    declare_parameter<std::string>("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<SegmentImage>(
+      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<const SegmentImage::Goal> goal)
+  {
+    (void)goal;
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleSegmentImage>)
+  {
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void handle_accepted(const std::shared_ptr<GoalHandleSegmentImage> goal_handle)
+  {
+    auto feedback = std::make_shared<SegmentImage::Feedback>();
+    feedback->current_state = "dummy_complete";
+    goal_handle->publish_feedback(feedback);
+
+    auto result = std::make_shared<SegmentImage::Result>();
+    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<SegmentImage>::SharedPtr action_server_;
+};
+
+}  // namespace nav2_segmentation
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_segmentation::SegmentationServer>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}