Implement generic C++ robot_controller

src/robot_controller/CMakeLists.txt

@@ -7,9 +7,8 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(robot_interfaces REQUIRED)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -22,16 +21,11 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(robot_interfaces REQUIRED)
-
-#add_executable(robotcontroller src/robotcontroller.cpp)
-#ament_target_dependencies(robotcontroller rclcpp robot_interfaces)
+add_executable(robot_controller src/cpp/robot_controller.cpp)
+ament_target_dependencies(robot_controller rclcpp robot_interfaces)
 
 install(TARGETS
-  talker
-  listener
+  robot_controller
   DESTINATION lib/${PROJECT_NAME})
 
 ament_package()

src/robot_controller/package.xml

@@ -9,6 +9,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>robot_interfaces</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/robot_controller/src/cpp/robot_controller.cpp

@@ -0,0 +1,65 @@
+#include <cstdio>
+#include "rclcpp/rclcpp.hpp"
+#include "robot_interfaces/srv/execute_motion.hpp"
+
+class RobotController : public rclcpp::Node
+{
+public:
+  /// Constructor
+  RobotController(std::string name)
+  : Node(name)
+  {
+  //this->service = this->create_service<robot_interfaces::srv::ExecuteMotion>(name+"_execute_path", &this->executePath);
+  this->service = this->create_service<robot_interfaces::srv::ExecuteMotion>(name+"_execute_path", std::bind(&RobotController::executePath, this, std::placeholders::_1, std::placeholders::_2));
+
+  // Subscribe to target pose
+  //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
+
+  //RCLCPP_INFO(this->get_logger(), "Initialization successful.");
+
+
+  //std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");
+
+  //rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service = node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);
+
+  //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");
+
+  //rclcpp::spin(node);
+  //rclcpp::shutdown();
+  }
+
+  //int (RobotController::*executePath)(const std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Request> request, std::shared_ptr<robot_interfaces::srv::ExecuteMotion::Response> response);
+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(), "executePath not implemented");
+}
+
+private:
+  /// Callback that executes path on robot
+
+  rclcpp::Service<robot_interfaces::srv::ExecuteMotion>::SharedPtr service;
+};
+
+
+//RobotController::RobotController(name) : Node(name)
+
+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::executors::SingleThreadedExecutor executor;
+  executor.add_node(robot);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

src/robot_controller/src/robotcontroller.cpp

@@ -1,41 +0,0 @@
-#include <cstdio>
-#include <rclcpp/rclcpp.hpp>
-
-class RobotController : public rclcpp::Node
-{
-public:
-  /// Constructor
-  RobotController();
-
-private:
-  /// Callback that executes path on robot
-    virtual void add(const std::shared_ptr<robot_interfaces::srv::AddThreeInts::Request> request,
-                           std::shared_ptr<robot_interfaces::srv::AddThreeInts::Response>       response);
-};
-
-RobotController::RobotController(string name) : Node(name)
-{
-  // Subscribe to target pose
-  target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
-
-  RCLCPP_INFO(this->get_logger(), "Initialization successful.");
-}
-
-int main(int argc, char ** argv)
-{
-  //rclcpp::init(argc, argv);
-
-  //auto target_follower = std::make_shared<MoveItFollowTarget>();
-
-  //rclcpp::executors::SingleThreadedExecutor executor;
-  //executor.add_node(target_follower);
-  //executor.spin();
-
-  //rclcpp::shutdown();
-  //return EXIT_SUCCESS;
-  (void) argc;
-  (void) argv;
-
-  printf("hello world\n");
-  return 0;
-}