Improve motion service

src/robot_controller/src/cpp/robot_controller.cpp

@@ -46,6 +46,27 @@ private:
   rclcpp::Service<robot_interfaces::srv::ExecuteMotion>::SharedPtr service;
 };
 
+// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
+// CRTP pattern used here https://en.wikipedia.org/wiki/Curiously_recurring_template_pattern
+//class DummyController : public RobotController<DummyController>
+class DummyController : public RobotController
+{
+  public:
+
+    DummyController(std::string name) : RobotController(name) {}
+
+    void executePath(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response) const
+    {
+      //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");
+    }
+
+};
 
 //RobotController::RobotController(name) : Node(name)
 
@@ -53,8 +74,8 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
 
-  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting generic robot_controller");
-  auto robot = std::make_shared<RobotController>("generic");
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting dummy_controller");
+  auto robot = std::make_shared<DummyController>("dummy");
 
   rclcpp::executors::SingleThreadedExecutor executor;
   executor.add_node(robot);