simple service call

Signed-off-by: M. Fatih Cırıt <[email protected]>

common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp

@@ -18,6 +18,8 @@
 #include "autoware_control_center/visibility_control.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
+#include "autoware_control_center_msgs/srv/autoware_node_register.hpp"
+
 namespace autoware_control_center
 {
 
@@ -27,6 +29,13 @@ class AutowareControlCenter : public rclcpp_lifecycle::LifecycleNode
   explicit AutowareControlCenter(const rclcpp::NodeOptions & options);
 
 private:
+  rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_;
+
+  rclcpp::Service<autoware_control_center_msgs::srv::AutowareNodeRegister>::SharedPtr srv_register_;
+
+  void register_node(
+    const autoware_control_center_msgs::srv::AutowareNodeRegister::Request::SharedPtr request,
+    const autoware_control_center_msgs::srv::AutowareNodeRegister::Response::SharedPtr response);
 };
 
 }  // namespace autoware_control_center

common/autoware_control_center/src/autoware_control_center.cpp

@@ -21,6 +21,24 @@ AutowareControlCenter::AutowareControlCenter(const rclcpp::NodeOptions & options
 {
   // log info
   RCLCPP_INFO(get_logger(), "AutowareControlCenter is initialized");
+
+  callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+  srv_register_ = create_service<autoware_control_center_msgs::srv::AutowareNodeRegister>(
+    "~/srv/autoware_node_register", std::bind(&AutowareControlCenter::register_node, this, _1, _2),
+    rmw_qos_profile_services_default, callback_group_mut_ex_);
+}
+
+void AutowareControlCenter::register_node(
+  const autoware_control_center_msgs::srv::AutowareNodeRegister::Request::SharedPtr request,
+  const autoware_control_center_msgs::srv::AutowareNodeRegister::Response::SharedPtr response)
+{
+  RCLCPP_INFO(get_logger(), "register_node is called from %s", request->name_node.c_str());
+
+  response->status.status =
+    autoware_control_center_msgs::srv::AutowareNodeRegister::Response::_status_type::SUCCESS;
 }
 
 }  // namespace autoware_control_center

common/autoware_node/src/autoware_node.cpp

@@ -25,10 +25,33 @@ AutowareNode::AutowareNode(
   const std::string & node_name, const std::string & ns, const rclcpp::NodeOptions & options)
 : LifecycleNode(node_name, ns, options)
 {
+  RCLCPP_INFO(get_logger(), "AutowareNode::AutowareNode()");
   callback_group_mut_ex_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
   cli_register_ = create_client<autoware_control_center_msgs::srv::AutowareNodeRegister>(
-    "~/cli/autoware_node_register", rmw_qos_profile_default, callback_group_mut_ex_);
+    "/autoware_control_center/srv/autoware_node_register", rmw_qos_profile_default,
+    callback_group_mut_ex_);
+
+  autoware_control_center_msgs::srv::AutowareNodeRegister::Request::SharedPtr req =
+    std::make_shared<autoware_control_center_msgs::srv::AutowareNodeRegister::Request>();
+
+  req->name_node = node_name;
+
+  auto fut_and_id_response = cli_register_->async_send_request(req);
+
+  RCLCPP_INFO(get_logger(), "Sent request");
+
+//  const auto & response = fut_and_id_response.get();
+//  RCLCPP_INFO(get_logger(), "response: %d", response->status.status);
+
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), fut_and_id_response)
+    ==
+        rclcpp::FutureReturnCode::SUCCESS)
+    {
+      RCLCPP_INFO(get_logger(), "response: %d", fut_and_id_response.get()->status.status);
+    } else {
+      RCLCPP_ERROR(get_logger(), "Failed to call service");
+    }
 }
 
 }  // namespace autoware_node

common/test_node/src/test_node.cpp

@@ -22,6 +22,7 @@ namespace test_node
 TestNode::TestNode(const rclcpp::NodeOptions & options)
 : autoware_node::AutowareNode("test_node", "", options)
 {
+  RCLCPP_INFO(get_logger(), "TestNode constructor");
 }
 
 }  // namespace test_node