✨ gazebo mark

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
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include>")
+
+# 链接依赖
+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() 

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`服务可用
+- 标记是静态物体，不会受到物理引擎影响
+- 每次请求会创建一个新的标记，标记会持续存在直到仿真结束 

test/cy-mark/include/cyberdog_marker/marker_service.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <memory>
+#include <string>
+#include <sstream>
+
+#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<cyberdog_marker::srv::PlaceMarker::Request> request,
+    std::shared_ptr<cyberdog_marker::srv::PlaceMarker::Response> response);
+
+  std::string generate_marker_sdf(const std::string & color, double radius = 0.05);
+  
+  rclcpp::Service<cyberdog_marker::srv::PlaceMarker>::SharedPtr place_marker_service_;
+  rclcpp::Client<gazebo_msgs::srv::SpawnEntity>::SharedPtr spawn_entity_client_;
+  
+  int marker_count_;
+};
+
+}  // namespace marker
+}  // namespace cyberdog 

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

test/cy-mark/model_marker.sdf

@@ -0,0 +1,21 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="marker">
+    <static>true</static>
+    <link name="link">
+      <visual name="visual">
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <sphere>
+            <radius>0.05</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>1 0 0 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf> 

test/cy-mark/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cyberdog_marker</name>
+  <version>0.0.1</version>
+  <description>Cyberdog marker placement service</description>
+  <maintainer email="user@example.com">user</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>gazebo_msgs</depend>
+  
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package> 

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<cyberdog_marker::srv::PlaceMarker>(
+    "place_marker",
+    std::bind(
+      &MarkerService::handle_place_marker, this,
+      std::placeholders::_1, std::placeholders::_2));
+      
+  // 创建客户端连接到Gazebo的spawn_entity服务
+  spawn_entity_client_ = create_client<gazebo_msgs::srv::SpawnEntity>("spawn_entity");
+  
+  RCLCPP_INFO(get_logger(), "Marker service started");
+}
+
+void MarkerService::handle_place_marker(
+  const std::shared_ptr<cyberdog_marker::srv::PlaceMarker::Request> request,
+  std::shared_ptr<cyberdog_marker::srv::PlaceMarker::Response> 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<gazebo_msgs::srv::SpawnEntity::Request>();
+  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 << "<?xml version='1.0'?>\n"
+      << "<sdf version='1.6'>\n"
+      << "  <model name='marker'>\n"
+      << "    <static>true</static>\n"
+      << "    <link name='link'>\n"
+      << "      <visual name='visual'>\n"
+      << "        <cast_shadows>false</cast_shadows>\n"
+      << "        <geometry>\n"
+      << "          <sphere>\n"
+      << "            <radius>" << radius << "</radius>\n"
+      << "          </sphere>\n"
+      << "        </geometry>\n"
+      << "        <material>\n"
+      << "          <ambient>" << r << " " << g << " " << b << " 1</ambient>\n"
+      << "          <diffuse>" << r << " " << g << " " << b << " 1</diffuse>\n"
+      << "          <specular>0.1 0.1 0.1 1</specular>\n"
+      << "        </material>\n"
+      << "      </visual>\n"
+      << "    </link>\n"
+      << "  </model>\n"
+      << "</sdf>";
+  
+  return sdf.str();
+}
+
+}  // namespace marker
+}  // namespace cyberdog
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(cyberdog::marker::MarkerService) 

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 # 返回的消息