add callbacks and multihthreaded executor

Signed-off-by: Aleksander Szymański <[email protected]>
LeoRover · Dec 28, 2023 · 9dfa6de · 9dfa6de
1 parent 6c1be61
commit 9dfa6de
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 5 deletions.
diff --git a/leo_fw/CMakeLists.txt b/leo_fw/CMakeLists.txt
@@ -32,6 +32,7 @@ ament_target_dependencies(leo_fw
 rclcpp_components_register_node(leo_fw
   PLUGIN "leo_fw::FirmwareMessageConverter"
   EXECUTABLE "firmware_message_converter"
+  EXECUTOR "MultiThreadedExecutor"
 )
 
 ament_python_install_package(${PROJECT_NAME})

diff --git a/leo_fw/src/firmware_message_converter.cpp b/leo_fw/src/firmware_message_converter.cpp
@@ -61,7 +61,8 @@ class FirmwareMessageConverter : public rclcpp::Node
         &FirmwareMessageConverter::set_imu_calibration_callback, this,
         std::placeholders::_1, std::placeholders::_2));
 
-    reset_odometry_client = create_client<std_srvs::srv::Trigger>("firmware/reset_odometry");
+    auto client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+    reset_odometry_client = create_client<std_srvs::srv::Trigger>("firmware/reset_odometry", rmw_qos_profile_services_default, client_cb_group_);
 
     reset_odometry_service = create_service<std_srvs::srv::Trigger>(
       "reset_odometry",
@@ -252,13 +253,15 @@ class FirmwareMessageConverter : public rclcpp::Node
 
     auto reset_request = std::make_shared<std_srvs::srv::Trigger_Request>();
 
-    auto result = reset_odometry_client->async_send_request(reset_request);
+    auto future = reset_odometry_client->async_send_request(reset_request);
 
-    auto result_status = result.wait_for(callback_timeout);
+    auto result_status = future.wait_for(callback_timeout);
 
-    if (result_status == std::future_status::ready) {
-      res->success = true;
+    if (future.valid()) {
+      res->success = future.get()->success;
     } else {
+      if(result_status == std::future_status::timeout)
+        res->message = "Firmware service timeout";
       res->success = false;
     }
   }