Reimplement robot_controller using ROS2 action

src/robot_controller/CMakeLists.txt

@@ -1,29 +1,31 @@
 cmake_minimum_required(VERSION 3.8)
 project(robot_controller)
 
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(robot_interfaces REQUIRED)
 
+add_executable(robot_controller src/cpp/robot_controller.cpp)
+ament_target_dependencies(robot_controller
+  "rclcpp"
+  "rclcpp_action"
+  "robot_interfaces")
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-add_executable(robot_controller src/cpp/robot_controller.cpp)
-ament_target_dependencies(robot_controller rclcpp robot_interfaces)
-
 install(TARGETS
   robot_controller
   DESTINATION lib/${PROJECT_NAME})

src/robot_controller/package.xml

@@ -10,6 +10,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
   <depend>robot_interfaces</depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/robot_controller/src/cpp/robot_controller.cpp

@@ -1,58 +1,125 @@
 #include <cstdio>
-#include "rclcpp/rclcpp.hpp"
-#include "robot_interfaces/srv/execute_motion.hpp"
+#include <functional>
+#include <memory>
+#include <thread>
 
+#include "robot_interfaces/action/execute_motion.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+//#include "robot_interfaces/visibility_control.h"
+
+//
+// https://docs.ros.org/en/foxy/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html
 class RobotController : public rclcpp::Node
 {
 public:
-  RobotController(std::string name) : Node(name)
-  {
-    this->service = this->create_service<robot_interfaces::srv::ExecuteMotion>("execute_path", std::bind(&RobotController::executePath, this, std::placeholders::_1, std::placeholders::_2));
-  }
+  using ExecuteMotion = robot_interfaces::action::ExecuteMotion;
+  using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle<ExecuteMotion>;
 
-  /// Callback that executes path on robot
-  virtual void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response)
+  explicit RobotController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+  : Node("robot_controller",options)
   {
-    //request->motion->path PoseStamped[]
-    //request->motion->acceleration float64
-    //request->motion->velocity float64
-    RCLCPP_INFO(this->get_logger(), "NEW MOTION: Acceleration: ");
-    //RCLCPP_INFO(this->get_logger(), "Acceleration: " + std::to_string(request.motion.acceleration));
-    response->status = "executePath not implemented";
-    RCLCPP_WARN(this->get_logger(), "executePath not implemented");
+    using namespace std::placeholders;
+
+    this->action_server_ = rclcpp_action::create_server<ExecuteMotion>(
+      this,
+      "execute_motion",
+      std::bind(&RobotController::motion_handle_goal, this, _1, _2),
+      std::bind(&RobotController::motion_handle_cancel, this, _1),
+      std::bind(&RobotController::motion_handle_accepted, this, _1));
   }
 
 private:
-  rclcpp::Service<robot_interfaces::srv::ExecuteMotion>::SharedPtr service;
+  rclcpp_action::Server<ExecuteMotion>::SharedPtr action_server_;
+
+  virtual rclcpp_action::GoalResponse motion_handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const ExecuteMotion::Goal> goal)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received goal request with acceleration %f", goal->motion.acceleration);
+    (void)uuid;
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  virtual rclcpp_action::CancelResponse motion_handle_cancel(
+    const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  virtual void motion_handle_accepted(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
+  {
+    using namespace std::placeholders;
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    std::thread{std::bind(&RobotController::executePath, this, _1), goal_handle}.detach();
+  }
+
+  /// Callback that executes path on robot
+  virtual void executePath(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
+  {
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<ExecuteMotion::Feedback>();
+    auto result = std::make_shared<ExecuteMotion::Result>();
+    std::string msg = "executePath not implemented";
+    result->result = msg;
+    feedback->status = msg;
+    RCLCPP_WARN(this->get_logger(), msg.c_str());
+  }
 };
 
 // A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
 class DummyController : public RobotController
 {
   public:
+  DummyController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options) {}
 
-    DummyController(std::string name) : RobotController(name) {}
+  /// Callback that executes path on robot
+  virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Executing goal");
+    rclcpp::Rate loop_rate(1);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
+    auto status = feedback->status;
+    auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
 
-    virtual void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response)
-    {
-      //request->motion->path PoseStamped[]
-      //request->motion->acceleration float64
-      //request->motion->velocity float64
-      RCLCPP_INFO(this->get_logger(), "NEW MOTION: Acceleration: ");
-      //RCLCPP_INFO(this->get_logger(), "Acceleration: " + std::to_string(request.motion.acceleration));
-      response->status = "executePath not implemented";
-      RCLCPP_WARN(this->get_logger(), "AAAAAA executePath not implemented");
+
+    for (int i = 1; (i < 100) && rclcpp::ok(); ++i) {
+      // Check if there is a cancel request
+      if (goal_handle->is_canceling()) {
+        result->result = status;
+        goal_handle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "Goal canceled");
+        return;
+      }
+      // Update status
+      status = i + "/100 complete";
+      // Publish feedback
+      goal_handle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), "Publish feedback");
+
+      loop_rate.sleep();
     }
 
+    // Check if goal is done
+    if (rclcpp::ok()) {
+      result->result = status;
+      goal_handle->succeed(result);
+      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+    }
+  }
 };
 
-
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
 
   RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting dummy_controller");
-  auto robot = std::make_shared<DummyController>("dummy");
+  auto robot = std::make_shared<DummyController>();
 
   rclcpp::executors::SingleThreadedExecutor executor;
   executor.add_node(robot);