diff --git a/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp b/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp index 4494d480..2ce895d5 100644 --- a/common/autoware_control_center/include/autoware_control_center/autoware_control_center.hpp +++ b/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::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 diff --git a/common/autoware_control_center/src/autoware_control_center.cpp b/common/autoware_control_center/src/autoware_control_center.cpp index 0b9605cd..04f97e04 100644 --- a/common/autoware_control_center/src/autoware_control_center.cpp +++ b/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( + "~/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 diff --git a/common/autoware_node/src/autoware_node.cpp b/common/autoware_node/src/autoware_node.cpp index 5775464b..e36fe195 100644 --- a/common/autoware_node/src/autoware_node.cpp +++ b/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( - "~/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(); + + 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 diff --git a/common/test_node/src/test_node.cpp b/common/test_node/src/test_node.cpp index 12faab80..ccceca94 100644 --- a/common/test_node/src/test_node.cpp +++ b/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