diff --git a/test/cy-mark/model_marker.sdf b/test/cy-mark/marker.sdf similarity index 100% rename from test/cy-mark/model_marker.sdf rename to test/cy-mark/marker.sdf diff --git a/test/cy-mark/marker_client.py b/test/cy-mark/marker_client.py index 23397f8..a448fde 100755 --- a/test/cy-mark/marker_client.py +++ b/test/cy-mark/marker_client.py @@ -24,7 +24,7 @@ class MarkerClient(Node): def main(args=None): rclpy.init(args=args) client = MarkerClient() - response = client.send_request(1.0, 2.0, 0.5, 'red') + response = client.send_request(1.0, 1.0, 0.0, 'blue') client.get_logger().info( f'结果: {response.success}, 消息: {response.message}') client.destroy_node() diff --git a/test/cy-mark/src/marker_service.cpp b/test/cy-mark/src/marker_service.cpp index 4833c5d..b2e682d 100644 --- a/test/cy-mark/src/marker_service.cpp +++ b/test/cy-mark/src/marker_service.cpp @@ -27,7 +27,7 @@ void MarkerService::handle_place_marker( std::shared_ptr response) { // 等待Gazebo服务可用 - if (!spawn_entity_client_->wait_for_service(std::chrono::seconds(5))) { + if (!spawn_entity_client_->wait_for_service(std::chrono::seconds(1))) { // 缩短超时,快速失败 response->success = false; response->message = "Gazebo spawn_entity service not available"; RCLCPP_ERROR(get_logger(), "%s", response->message.c_str()); @@ -45,37 +45,45 @@ void MarkerService::handle_place_marker( auto spawn_request = std::make_shared(); spawn_request->name = marker_name; spawn_request->xml = marker_sdf; - spawn_request->robot_namespace = ""; + spawn_request->robot_namespace = ""; // 通常为空,除非你的Gazebo模型在特定命名空间下 // 设置标记位置 spawn_request->initial_pose.position.x = request->x; spawn_request->initial_pose.position.y = request->y; spawn_request->initial_pose.position.z = request->z; - spawn_request->initial_pose.orientation.w = 1.0; + spawn_request->initial_pose.orientation.w = 1.0; // 默认无旋转 // 发送请求 - auto future = spawn_entity_client_->async_send_request(spawn_request); + auto future_response = spawn_entity_client_->async_send_request(spawn_request); - // 等待响应 - if (rclcpp::spin_until_future_complete(shared_from_this(), future) == - rclcpp::FutureReturnCode::SUCCESS) - { - auto spawn_response = future.get(); - response->success = spawn_response->success; + // 让我们先尝试一个简单的阻塞获取,看看gazebo服务是否足够快 + std::chrono::seconds timeout(5); // 设置一个超时 + auto future_status = future_response.wait_for(timeout); + + if (future_status == std::future_status::ready) { + auto spawn_response_ptr = future_response.get(); + response->success = spawn_response_ptr->success; if (response->success) { response->message = "Successfully placed marker '" + marker_name + "' at (" + std::to_string(request->x) + ", " + std::to_string(request->y) + ", " + std::to_string(request->z) + ")"; } else { - response->message = "Failed to place marker: " + spawn_response->status_message; + response->message = "Failed to place marker: " + spawn_response_ptr->status_message; } - } else { + } else if (future_status == std::future_status::timeout) { response->success = false; - response->message = "Failed to call spawn_entity service"; + response->message = "Gazebo spawn_entity service call timed out"; + } else { // deferred + response->success = false; + response->message = "Gazebo spawn_entity service call deferred or error"; } - RCLCPP_INFO(get_logger(), "%s", response->message.c_str()); + if (response->success) { + RCLCPP_INFO(get_logger(), "%s", response->message.c_str()); + } else { + RCLCPP_ERROR(get_logger(), "%s", response->message.c_str()); + } } std::string MarkerService::generate_marker_sdf(const std::string & color, double radius)