ros2: update terrain_planner_ros

- Correct service client call. Signed-off-by: Rhys Mainwaring <[email protected]>
ethz-asl · Nov 15, 2023 · 9b996aa · 9b996aa
1 parent 2187980
commit 9b996aa
Showing 1 changed file with 21 additions and 21 deletions.
diff --git a/terrain_navigation_ros/src/terrain_planner.cpp b/terrain_navigation_ros/src/terrain_planner.cpp
@@ -314,20 +314,21 @@ void TerrainPlanner::cmdloopCallback() {
           image_capture_req->param7 = 0;  // sequence number
 
           while (!msginterval_serviceclient_->wait_for_service(1s)) {
-            if (!rclcpp::ok()) {
-              RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for service.");
-              return;
-            }
-            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Service not available, waiting ...");
+            RCLCPP_WARN_STREAM(this->get_logger(), "Service ["
+                << msginterval_serviceclient_->get_service_name()
+                << "] not available.");
+            return;
           }
 
           auto result = msginterval_serviceclient_->async_send_request(image_capture_req);
 
-          // if (rclcpp::spin_until_future_complete(node, result) !=
-          //     rclcpp::FutureReturnCode::SUCCESS)
-          // {
-          //   RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service.");
-          // }
+          if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS)
+          {
+            RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service ["
+                << msginterval_serviceclient_->get_service_name()
+                << "] failed.");
+            return;
+          }
         }
         /// TODO: Get reference attitude from path reference states
         const double pitch = std::atan(reference_tangent(2) / reference_tangent.head(2).norm());
@@ -402,23 +403,22 @@ void TerrainPlanner::plannerloopCallback() {
     request_global_origin_req->command = mavros_msgs::msg::CommandCode::REQUEST_MESSAGE;
     request_global_origin_req->param1 = 49;
 
-    // msginterval_serviceclient_.call(request_global_origin_msg);
     while (!msginterval_serviceclient_->wait_for_service(1s)) {
-      if (!rclcpp::ok()) {
-        RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for service.");
+        RCLCPP_WARN_STREAM(this->get_logger(), "Service ["
+            << msginterval_serviceclient_->get_service_name()
+            << "] not available.");
         return;
-      }
-      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Service not available, waiting ...");
     }
 
     auto result = msginterval_serviceclient_->async_send_request(request_global_origin_req);
 
-    // if (rclcpp::spin_until_future_complete(node, result) !=
-    //     rclcpp::FutureReturnCode::SUCCESS)
-    // {
-    //   RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service.");
-    // }
-
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS)
+    {
+      RCLCPP_ERROR_STREAM(this->get_logger(), "Call to service ["
+          << msginterval_serviceclient_->get_service_name()
+          << "] failed.");
+      return;
+    }
     return;
   }