diff --git a/test/cy-mark/CMakeLists.txt b/test/cy-mark/CMakeLists.txt new file mode 100644 index 0000000..c2b9c79 --- /dev/null +++ b/test/cy-mark/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.8) +project(cyberdog_marker) + +# 编译选项 +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# 查找依赖项 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(gazebo_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# 生成服务接口 +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/PlaceMarker.srv" + DEPENDENCIES +) + +# 设置服务接口的链接 +rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") + +# 添加库 +add_library(marker_service SHARED + src/marker_service.cpp +) + +# 设置包含路径 +target_include_directories(marker_service PUBLIC + "$" + "$") + +# 链接依赖 +target_link_libraries(marker_service + "${cpp_typesupport_target}" +) + +ament_target_dependencies(marker_service + rclcpp + rclcpp_components + gazebo_msgs +) + +# 安装头文件 +install( + DIRECTORY include/ + DESTINATION include +) + +# 安装库 +install(TARGETS + marker_service + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# 组件导出 +rclcpp_components_register_node( + marker_service + PLUGIN "cyberdog::marker::MarkerService" + EXECUTABLE marker_service_node +) + +ament_package() \ No newline at end of file diff --git a/test/cy-mark/README.md b/test/cy-mark/README.md new file mode 100644 index 0000000..307d5bf --- /dev/null +++ b/test/cy-mark/README.md @@ -0,0 +1,74 @@ +# Cyberdog 标记服务 + +这个包实现了在Gazebo模拟环境中动态添加标记点的功能。 + +## 简介 + +`cyberdog_marker`包提供了一个ROS2服务，允许在仿真运行过程中在指定的位置放置一个标记。标记可以是不同颜色的球体，便于在仿真环境中标记特定的位置。 + +## 安装 + +将本包复制到您的ROS2工作空间的`src`目录下，例如： + +```bash +cp -r cy-mark ~/your_workspace/src/cyberdog_marker +``` + +然后编译： + +```bash +cd ~/your_workspace +colcon build --merge-install --symlink-install --packages-select cyberdog_marker +source install/setup.bash +``` + +## 使用方法 + +### 启动服务 + +```bash +ros2 run cyberdog_marker marker_service_node +``` + +### 使用服务 + +通过ROS2服务调用放置标记： + +```bash +ros2 service call /place_marker cyberdog_marker/srv/PlaceMarker "{x: 1.0, y: 2.0, z: 0.5, color: 'red'}" +``` + +参数说明： +- `x`, `y`, `z`: 标记的位置坐标 +- `color`: 标记的颜色，支持 "red", "green", "blue", "yellow" + +### 使用Python客户端 + +也可以使用提供的Python客户端脚本： + +```bash +# 确保脚本有执行权限 +chmod +x marker_client.py +# 运行客户端 +./marker_client.py +``` + +## 服务接口 + +### 请求 + +- `float64 x`: X坐标 +- `float64 y`: Y坐标 +- `float64 z`: Z坐标 +- `string color`: 颜色（"red", "green", "blue", "yellow"） + +### 响应 + +- `bool success`: 操作是否成功 +- `string message`: 返回的消息 + +## 注意事项 + +- 确保Gazebo已经运行并且`/spawn_entity`服务可用 +- 标记是静态物体，不会受到物理引擎影响 +- 每次请求会创建一个新的标记，标记会持续存在直到仿真结束 \ No newline at end of file diff --git a/test/cy-mark/include/cyberdog_marker/marker_service.hpp b/test/cy-mark/include/cyberdog_marker/marker_service.hpp new file mode 100644 index 0000000..e219f28 --- /dev/null +++ b/test/cy-mark/include/cyberdog_marker/marker_service.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "gazebo_msgs/srv/spawn_entity.hpp" +#include "cyberdog_marker/srv/place_marker.hpp" + +namespace cyberdog +{ +namespace marker +{ + +class MarkerService : public rclcpp::Node +{ +public: + explicit MarkerService(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~MarkerService() = default; + +private: + void handle_place_marker( + const std::shared_ptr request, + std::shared_ptr response); + + std::string generate_marker_sdf(const std::string & color, double radius = 0.05); + + rclcpp::Service::SharedPtr place_marker_service_; + rclcpp::Client::SharedPtr spawn_entity_client_; + + int marker_count_; +}; + +} // namespace marker +} // namespace cyberdog \ No newline at end of file diff --git a/test/cy-mark/marker_client.py b/test/cy-mark/marker_client.py new file mode 100755 index 0000000..23397f8 --- /dev/null +++ b/test/cy-mark/marker_client.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from cyberdog_marker.srv import PlaceMarker + +class MarkerClient(Node): + def __init__(self): + super().__init__('marker_client') + self.client = self.create_client(PlaceMarker, 'place_marker') + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('服务不可用，等待...') + self.req = PlaceMarker.Request() + + def send_request(self, x, y, z, color): + self.req.x = x + self.req.y = y + self.req.z = z + self.req.color = color + self.future = self.client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result() + +def main(args=None): + rclpy.init(args=args) + client = MarkerClient() + response = client.send_request(1.0, 2.0, 0.5, 'red') + client.get_logger().info( + f'结果: {response.success}, 消息: {response.message}') + client.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/test/cy-mark/model_marker.sdf b/test/cy-mark/model_marker.sdf new file mode 100644 index 0000000..1e7b543 --- /dev/null +++ b/test/cy-mark/model_marker.sdf @@ -0,0 +1,21 @@ + + + + true + + + false + + + 0.05 + + + + 1 0 0 1 + 1 0 0 1 + 0.1 0.1 0.1 1 + + + + + \ No newline at end of file diff --git a/test/cy-mark/package.xml b/test/cy-mark/package.xml new file mode 100644 index 0000000..89597e8 --- /dev/null +++ b/test/cy-mark/package.xml @@ -0,0 +1,23 @@ + + + + cyberdog_marker + 0.0.1 + Cyberdog marker placement service + user + Apache License 2.0 + + ament_cmake + + rclcpp + rclcpp_components + gazebo_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/test/cy-mark/src/marker_service.cpp b/test/cy-mark/src/marker_service.cpp new file mode 100644 index 0000000..4833c5d --- /dev/null +++ b/test/cy-mark/src/marker_service.cpp @@ -0,0 +1,126 @@ +#include "cyberdog_marker/marker_service.hpp" + +namespace cyberdog +{ +namespace marker +{ + +MarkerService::MarkerService(const rclcpp::NodeOptions & options) +: Node("cyberdog_marker_service", options), + marker_count_(0) +{ + // 创建服务提供位置标记功能 + place_marker_service_ = create_service( + "place_marker", + std::bind( + &MarkerService::handle_place_marker, this, + std::placeholders::_1, std::placeholders::_2)); + + // 创建客户端连接到Gazebo的spawn_entity服务 + spawn_entity_client_ = create_client("spawn_entity"); + + RCLCPP_INFO(get_logger(), "Marker service started"); +} + +void MarkerService::handle_place_marker( + const std::shared_ptr request, + std::shared_ptr response) +{ + // 等待Gazebo服务可用 + if (!spawn_entity_client_->wait_for_service(std::chrono::seconds(5))) { + response->success = false; + response->message = "Gazebo spawn_entity service not available"; + RCLCPP_ERROR(get_logger(), "%s", response->message.c_str()); + return; + } + + // 生成唯一的标记名称 + marker_count_++; + std::string marker_name = "marker_" + std::to_string(marker_count_); + + // 生成SDF标记内容 + std::string marker_sdf = generate_marker_sdf(request->color); + + // 创建spawn_entity请求 + auto spawn_request = std::make_shared(); + spawn_request->name = marker_name; + spawn_request->xml = marker_sdf; + spawn_request->robot_namespace = ""; + + // 设置标记位置 + 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; + + // 发送请求 + auto future = 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; + 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; + } + } else { + response->success = false; + response->message = "Failed to call spawn_entity service"; + } + + RCLCPP_INFO(get_logger(), "%s", response->message.c_str()); +} + +std::string MarkerService::generate_marker_sdf(const std::string & color, double radius) +{ + // 解析颜色 + double r = 1.0, g = 0.0, b = 0.0; // 默认红色 + if (color == "red") { + r = 1.0; g = 0.0; b = 0.0; + } else if (color == "green") { + r = 0.0; g = 1.0; b = 0.0; + } else if (color == "blue") { + r = 0.0; g = 0.0; b = 1.0; + } else if (color == "yellow") { + r = 1.0; g = 1.0; b = 0.0; + } + + // 创建SDF模型 + std::stringstream sdf; + sdf << "\n" + << "\n" + << " \n" + << " true\n" + << " \n" + << " \n" + << " false\n" + << " \n" + << " \n" + << " " << radius << "\n" + << " \n" + << " \n" + << " \n" + << " " << r << " " << g << " " << b << " 1\n" + << " " << r << " " << g << " " << b << " 1\n" + << " 0.1 0.1 0.1 1\n" + << " \n" + << " \n" + << " \n" + << " \n" + << ""; + + return sdf.str(); +} + +} // namespace marker +} // namespace cyberdog + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(cyberdog::marker::MarkerService) \ No newline at end of file diff --git a/test/cy-mark/srv/PlaceMarker.srv b/test/cy-mark/srv/PlaceMarker.srv new file mode 100644 index 0000000..c7f8122 --- /dev/null +++ b/test/cy-mark/srv/PlaceMarker.srv @@ -0,0 +1,9 @@ +# 请求消息：标记的位置和类型 +float64 x # x坐标 +float64 y # y坐标 +float64 z # z坐标 +string color # 标记颜色，例如 "red", "green", "blue" +--- +# 响应消息 +bool success # 标记是否成功放置 +string message # 返回的消息 \ No newline at end of file