Add ActionServer (#9)

* More verbose error message when trying to assign a value to a CompoundMessage.

* Moved BabelFishAction definition to separate header in preparation for BabelFishActionServer.

* Added BabelFishActionServer with tests.

* Suppress false positive in cppcheck.

* Enable inline suppression in cppcheck.

.github/workflows/lint-and-test.yaml

@@ -24,7 +24,7 @@ jobs:
           clang-format -Werror -n -style=file $(find . -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -o -name '*.c' -o -name '*.cc')
       - name: Check C++
         run: |
-          cppcheck --force --quiet --error-exitcode=1 .
+          cppcheck --force --quiet --inline-suppr --error-exitcode=1 .
       - name: Lint package.xml
         run: |
           xmllint --noout --schema http://download.ros.org/schema/package_format3.xsd $(find . -name 'package.xml')
@@ -59,4 +59,4 @@ jobs:
         run: |
           source /opt/ros/${{ matrix.setup.rosdistro }}/setup.bash
           colcon test
-          colcon test-result --verbose
+          colcon test-result --verbose

ros_babel_fish/CMakeLists.txt

@@ -30,6 +30,7 @@ find_package(rosidl_typesupport_introspection_cpp REQUIRED)
 
 set(SOURCES
   src/detail/babel_fish_action_client.cpp
+  src/detail/babel_fish_action_server.cpp
   src/detail/babel_fish_publisher.cpp
   src/detail/babel_fish_service.cpp
   src/detail/babel_fish_service_client.cpp
@@ -155,6 +156,10 @@ if (BUILD_TESTING)
   ament_add_gtest(test_action_client test/action_client.cpp)
   ament_target_dependencies(test_action_client example_interfaces ros_babel_fish_test_msgs)
   target_link_libraries(test_action_client ${PROJECT_NAME})
+
+  ament_add_gtest(test_action_server test/action_server.cpp)
+  ament_target_dependencies(test_action_server example_interfaces ros_babel_fish_test_msgs)
+  target_link_libraries(test_action_server ${PROJECT_NAME})
 endif ()
 
 # to run: catkin build ros_babel_fish --no-deps -DENABLE_COVERAGE_TESTING=ON -DCMAKE_BUILD_TYPE=Debug -v --catkin-make-args ros_babel_fish_coverage

ros_babel_fish/include/ros_babel_fish/babel_fish.hpp

@@ -5,6 +5,7 @@
 #define ROS_BABEL_FISH_BABEL_FISH_HPP
 
 #include "ros_babel_fish/detail/babel_fish_action_client.hpp"
+#include "ros_babel_fish/detail/babel_fish_action_server.hpp"
 #include "ros_babel_fish/detail/babel_fish_publisher.hpp"
 #include "ros_babel_fish/detail/babel_fish_service.hpp"
 #include "ros_babel_fish/detail/babel_fish_service_client.hpp"
@@ -150,6 +151,23 @@ class BabelFish : public std::enable_shared_from_this<BabelFish>
                          const rmw_qos_profile_t &qos_profile = rmw_qos_profile_services_default,
                          rclcpp::CallbackGroup::SharedPtr group = nullptr );
 
+  /*!
+   * Creates an action server for the given name and type.
+   * @param name The name under which the action server is registered.
+   * @param type They type of the action.
+   * @param handle_goal Callback when a new goal was received.
+   * @param handle_cancel Callback when a cancel request was received.
+   * @param handle_accepted Callback when a goal was accepted. Should start executing the goal.
+   * @return An action server that goals can be sent to for processing.
+   */
+  BabelFishActionServer::SharedPtr create_action_server(
+      rclcpp::Node &node, const std::string &name, const std::string &type,
+      BabelFishActionServer::GoalCallback handle_goal,
+      BabelFishActionServer::CancelCallback handle_cancel,
+      BabelFishActionServer::AcceptedCallback handle_accepted,
+      const rcl_action_server_options_t &options = rcl_action_server_get_default_options(),
+      rclcpp::CallbackGroup::SharedPtr group = nullptr );
+
   /*!
    * Creates an action client for the given name and type.
    * @param name The name under which the action server is registered.

ros_babel_fish/include/ros_babel_fish/detail/babel_fish_action.hpp

@@ -0,0 +1,40 @@
+// Copyright (c) 2024 Stefan Fabian. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full license information.
+
+#ifndef ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP
+#define ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP
+
+#include "ros_babel_fish/messages/compound_message.hpp"
+
+namespace ros_babel_fish
+{
+struct ActionTypeSupport;
+
+namespace impl
+{
+struct BabelFishAction {
+  using Feedback = CompoundMessage;
+  using Goal = CompoundMessage;
+  using Result = CompoundMessage;
+
+  struct Impl {
+    struct SendGoalService {
+      using Request = CompoundMessage;
+      using Response = CompoundMessage;
+    };
+    struct GetResultService {
+      using Request = CompoundMessage;
+      using Response = CompoundMessage;
+    };
+    using FeedbackMessage = CompoundMessage;
+    struct CancelGoalService {
+      using Request = CompoundMessage;
+      using Response = CompoundMessage;
+    };
+    using GoalStatusMessage = CompoundMessage;
+  };
+};
+} // namespace impl
+} // namespace ros_babel_fish
+
+#endif // ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP

ros_babel_fish/include/ros_babel_fish/detail/babel_fish_action_client.hpp

@@ -4,30 +4,9 @@
 #ifndef ROS_BABEL_FISH_BABEL_FISH_ACTION_CLIENT_HPP
 #define ROS_BABEL_FISH_BABEL_FISH_ACTION_CLIENT_HPP
 
-#include "ros_babel_fish/messages/compound_message.hpp"
+#include "ros_babel_fish/detail/babel_fish_action.hpp"
 #include <rclcpp_action/client.hpp>
 
-namespace ros_babel_fish
-{
-struct ActionTypeSupport;
-
-namespace impl
-{
-struct BabelFishAction {
-  using Feedback = CompoundMessage;
-  using Goal = CompoundMessage;
-  using Result = CompoundMessage;
-
-  typedef struct Impl {
-    struct CancelGoalService {
-      using Request = CompoundMessage;
-      using Response = CompoundMessage;
-    };
-  } Impl;
-};
-} // namespace impl
-} // namespace ros_babel_fish
-
 namespace rclcpp_action
 {
 template<>

ros_babel_fish/include/ros_babel_fish/detail/babel_fish_action_server.hpp

@@ -1,33 +1,138 @@
 // Copyright (c) 2021 Stefan Fabian. All rights reserved.
 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
 
-#ifndef ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP
-#define ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP
+#ifndef ROS_BABEL_FISH_BABEL_FISH_ACTION_SERVER_HPP
+#define ROS_BABEL_FISH_BABEL_FISH_ACTION_SERVER_HPP
 
-#include <rclcpp/ac.hpp>
+#include "ros_babel_fish/detail/babel_fish_action.hpp"
+#include <rclcpp_action/server.hpp>
 #include <ros_babel_fish/idl/type_support.hpp>
 #include <ros_babel_fish/messages/compound_message.hpp>
 
-namespace ros_babel_fish
+namespace rclcpp_action
 {
 
-class BabelFishService
+template<>
+class ServerGoalHandle<ros_babel_fish::impl::BabelFishAction>
+    : public ServerGoalHandleBase,
+      public std::enable_shared_from_this<ServerGoalHandle<ros_babel_fish::impl::BabelFishAction>>
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS( BabelFishService )
+  using ActionT = ros_babel_fish::impl::BabelFishAction;
+
+  /// @see ServerGoalHandle<ActionT>::publish_feedback
+  void publish_feedback( const ActionT::Feedback &feedback_msg ) const;
+
+  /// @see ServerGoalHandle<ActionT>::abort
+  void abort( const ActionT::Result &result_msg );
+
+  /// @see ServerGoalHandle<ActionT>::succeed
+  void succeed( const ActionT::Result &result_msg );
+
+  // @see ServerGoalHandle<ActionT>::canceled
+  void canceled( const ActionT::Result &result_msg );
 
-  BabelFishService(
-      rclcpp::Node *node, const std::string &name, ServiceTypeSupport::ConstSharedPtr type_support,
-      std::function<void( const rmw_request_id_t &, const CompoundMessage &, CompoundMessage & )> callback,
-      rcl_service_options_t options );
+  /// @see ServerGoalHandle<ActionT>::execute
+  void execute();
 
-  rclcpp::ServiceBase::ConstSharedPtr getService() const;
+  /// Get the user provided message describing the goal.
+  const std::shared_ptr<const ActionT::Goal> &get_goal() const { return goal_; }
 
-  rclcpp::ServiceBase::SharedPtr getService();
+  /// Get the unique identifier of the goal
+  const GoalUUID &get_goal_id() const { return uuid_; }
+
+  ~ServerGoalHandle() override;
+
+  ActionT::Result create_result_message() const;
+
+  ActionT::Feedback create_feedback_message() const;
 
 private:
-  rclcpp::ServiceBase::SharedPtr service_;
+  ServerGoalHandle( ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support,
+                    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle, GoalUUID uuid,
+                    std::shared_ptr<const typename ActionT::Goal> goal,
+                    std::function<void( const GoalUUID &, std::shared_ptr<void> )> on_terminal_state,
+                    std::function<void( const GoalUUID & )> on_executing,
+                    std::function<void( std::shared_ptr<void> )> publish_feedback );
+
+  ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support_;
+  const std::shared_ptr<const ActionT::Goal> goal_;
+  const GoalUUID uuid_;
+
+  friend class Server<ActionT>;
+
+  std::function<void( const GoalUUID &, std::shared_ptr<void> )> on_terminal_state_;
+  std::function<void( const GoalUUID & )> on_executing_;
+  std::function<void( std::shared_ptr<void> )> publish_feedback_;
 };
+
+template<>
+class Server<ros_babel_fish::impl::BabelFishAction>
+    : public ServerBase,
+      public std::enable_shared_from_this<Server<ros_babel_fish::impl::BabelFishAction>>
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE( Server )
+  using ActionT = ros_babel_fish::impl::BabelFishAction;
+  using GoalHandle = ServerGoalHandle<ActionT>;
+
+  using GoalCallback =
+      std::function<GoalResponse( const GoalUUID &, std::shared_ptr<const ActionT::Goal> )>;
+
+  using CancelCallback =
+      std::function<CancelResponse( const std::shared_ptr<ServerGoalHandle<ActionT>> )>;
+
+  using AcceptedCallback = std::function<void( std::shared_ptr<ServerGoalHandle<ActionT>> )>;
+
+  //! Do not call directly, this is private API and might change. Use BabelFish::create_action_server.
+  Server( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+          rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+          rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+          const std::string &name, ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support,
+          const rcl_action_server_options_t &options, GoalCallback handle_goal,
+          CancelCallback handle_cancel, AcceptedCallback handle_accepted );
+
+  ~Server() override;
+
+private:
+  std::pair<GoalResponse, std::shared_ptr<void>>
+  call_handle_goal_callback( GoalUUID &uuid, std::shared_ptr<void> message ) override;
+
+  CancelResponse call_handle_cancel_callback( const GoalUUID &uuid ) override;
+
+  void call_goal_accepted_callback( std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
+                                    GoalUUID uuid,
+                                    std::shared_ptr<void> goal_request_message ) override;
+
+  GoalUUID get_goal_id_from_goal_request( void *message ) override;
+
+  std::shared_ptr<void> create_goal_request() override;
+
+  GoalUUID get_goal_id_from_result_request( void *message ) override;
+
+  std::shared_ptr<void> create_result_request() override;
+
+  std::shared_ptr<void>
+  create_result_response( decltype( action_msgs::msg::GoalStatus::status ) status ) override;
+
+  ros_babel_fish::ActionTypeSupport::ConstSharedPtr type_support_;
+
+  GoalCallback handle_goal_;
+  CancelCallback handle_cancel_;
+  AcceptedCallback handle_accepted_;
+
+  using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
+  /// A map of goal id to goal handle weak pointers.
+  /// This is used to provide a goal handle to handle_cancel.
+  std::unordered_map<GoalUUID, GoalHandleWeakPtr> goal_handles_;
+  std::mutex goal_handles_mutex_;
+};
+} // namespace rclcpp_action
+
+namespace ros_babel_fish
+{
+using BabelFishActionServer = rclcpp_action::Server<impl::BabelFishAction>;
+using BabelFishActionServerGoalHandle = rclcpp_action::ServerGoalHandle<impl::BabelFishAction>;
 } // namespace ros_babel_fish
 
 #endif // ROS_BABEL_FISH_BABEL_FISH_ACTION_HPP

ros_babel_fish/src/babel_fish.cpp

@@ -152,6 +152,55 @@ BabelFish::create_service_client( rclcpp::Node &node, const std::string &service
   return result;
 }
 
+BabelFishActionServer::SharedPtr BabelFish::create_action_server(
+    rclcpp::Node &node, const std::string &name, const std::string &type,
+    BabelFishActionServer::GoalCallback handle_goal,
+    BabelFishActionServer::CancelCallback handle_cancel,
+    BabelFishActionServer::AcceptedCallback handle_accepted,
+    const rcl_action_server_options_t &options, rclcpp::CallbackGroup::SharedPtr group )
+{
+  ActionTypeSupport::ConstSharedPtr type_support = get_action_type_support( type );
+  if ( type_support == nullptr ) {
+    throw BabelFishException( "Failed to create an action client for type: " + type +
+                              ". Type not found!" );
+  }
+  std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
+      node.get_node_waitables_interface();
+  std::weak_ptr<rclcpp::CallbackGroup> weak_group = group;
+  bool group_is_null = ( nullptr == group.get() );
+
+  auto deleter = [weak_node, weak_group, group_is_null]( BabelFishActionServer *ptr ) {
+    if ( nullptr == ptr ) {
+      return;
+    }
+    if ( auto shared_node = weak_node.lock(); shared_node != nullptr ) {
+      // API expects a shared pointer, give it one with a deleter that does nothing.
+      std::shared_ptr<BabelFishActionServer> fake_shared_ptr(
+          ptr, []( const BabelFishActionServer * ) { /* do nothing */ } );
+
+      if ( group_is_null ) {
+        // Was added to default group
+        shared_node->remove_waitable( fake_shared_ptr, nullptr );
+      } else {
+        // Was added to a specific group
+        auto shared_group = weak_group.lock();
+        if ( shared_group ) {
+          shared_node->remove_waitable( fake_shared_ptr, shared_group );
+        }
+      }
+    }
+    delete ptr;
+  };
+  std::shared_ptr<BabelFishActionServer> server(
+      new BabelFishActionServer( node.get_node_base_interface(), node.get_node_clock_interface(),
+                                 node.get_node_logging_interface(), name, type_support, options,
+                                 std::move( handle_goal ), std::move( handle_cancel ),
+                                 std::move( handle_accepted ) ),
+      deleter );
+  node.get_node_waitables_interface()->add_waitable( server, std::move( group ) );
+  return server;
+}
+
 BabelFishActionClient::SharedPtr
 BabelFish::create_action_client( rclcpp::Node &node, const std::string &name,
                                  const std::string &type, const rcl_action_client_options_t &options,
@@ -171,10 +220,10 @@ BabelFish::create_action_client( rclcpp::Node &node, const std::string &name,
     if ( nullptr == ptr ) {
       return;
     }
-    auto shared_node = weak_node.lock();
-    if ( shared_node ) {
+    if ( auto shared_node = weak_node.lock(); shared_node != nullptr ) {
       // API expects a shared pointer, give it one with a deleter that does nothing.
-      std::shared_ptr<BabelFishActionClient> fake_shared_ptr( ptr, []( BabelFishActionClient * ) {} );
+      std::shared_ptr<BabelFishActionClient> fake_shared_ptr(
+          ptr, []( const BabelFishActionClient * ) { /* not ours to delete */ } );
 
       if ( group_is_null ) {
         // Was added to default group