Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ finish mark;

Browse Source 
test(cy-mark): improve marker placement service and update test case

- Update marker_client.py to test with different coordinates and color
- Remove model_marker.sdf file as it's no longer needed
- Enhance marker_service.cpp with better error handling and logging
This commit is contained in:
havoc420ubuntu 2025-05-14 17:15:49 +00:00
parent ac97ca29e7
commit 30d02cba9d
3 changed files with 23 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
test/cy-mark/model_marker.sdf → test/cy-mark/marker.sdf
View File

2
test/cy-mark/marker_client.py
View File

@ -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()

36
test/cy-mark/src/marker_service.cpp
View File

@ -27,7 +27,7 @@ void MarkerService::handle_place_marker(
std::shared_ptr<cyberdog_marker::srv::PlaceMarker::Response> 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<gazebo_msgs::srv::SpawnEntity::Request>();
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)