diff --git a/.gitignore b/.gitignore index f3d48ea..95bfeed 100644 --- a/.gitignore +++ b/.gitignore @@ -122,7 +122,7 @@ devel_isolated/ CMakeLists.txt.user srv/_*.py -*.pcd + *.pyc qtcreator-* *.user @@ -156,7 +156,7 @@ build/ CMakeLists.txt.user srv/_*.py -*.pcd + *.pyc qtcreator-* *.user diff --git a/src/rm_decision/include/rm_decision/decision_node.hpp b/src/rm_decision/include/rm_decision/decision_node.hpp index 18e293b..d36608c 100644 --- a/src/rm_decision/include/rm_decision/decision_node.hpp +++ b/src/rm_decision/include/rm_decision/decision_node.hpp @@ -7,7 +7,6 @@ #include #include #include -#include #include #include #include diff --git a/src/rm_msg b/src/rm_msg deleted file mode 160000 index 40b1fcf..0000000 --- a/src/rm_msg +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 40b1fcf31d4b967bd1e6d94fe2b10e7ebb543ae9 diff --git a/src/rm_msgs/.gitignore b/src/rm_msgs/.gitignore new file mode 100644 index 0000000..35d74bb --- /dev/null +++ b/src/rm_msgs/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/src/rm_msgs/CMakeLists.txt b/src/rm_msgs/CMakeLists.txt new file mode 100644 index 0000000..a75997f --- /dev/null +++ b/src/rm_msgs/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(rm_msgs) + +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/DataMCU.msg" + "msg/DataRef.msg" + "msg/DataAI.msg" + "msg/DataAim.msg" + "msg/NavGoal.msg" + "msg/NavStatus.msg" +) + +ament_package() + diff --git a/src/rm_msgs/LICENSE b/src/rm_msgs/LICENSE new file mode 100644 index 0000000..cb386d0 --- /dev/null +++ b/src/rm_msgs/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2025 zucheng Lv + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/src/rm_msgs/README.md b/src/rm_msgs/README.md new file mode 100644 index 0000000..d7eb0ee --- /dev/null +++ b/src/rm_msgs/README.md @@ -0,0 +1,6 @@ +# rm_msg +Some ROS 2 custom messages for Robotaster + +Usage +Modify or add files in the /msg directory as needed +colcon build \ No newline at end of file diff --git a/src/rm_msgs/msg/DataAI.msg b/src/rm_msgs/msg/DataAI.msg new file mode 100644 index 0000000..81c7ee0 --- /dev/null +++ b/src/rm_msgs/msg/DataAI.msg @@ -0,0 +1,13 @@ +# AI 控制数据 (上位机 -> MCU) + +uint8 mode # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 +float32 yaw # 目标偏航角 +float32 yaw_vel # 偏航角速度 +float32 yaw_acc # 偏航角加速度 +float32 pitch # 目标俯仰角 +float32 pitch_vel # 俯仰角速度 +float32 pitch_acc # 俯仰角加速度 +float32 vx # X 方向速度 +float32 vy # Y 方向速度 +float32 wz # Z 方向角速度 +uint8 reserved # 预留字段（用于传递底盘模式等） diff --git a/src/rm_msgs/msg/DataAim.msg b/src/rm_msgs/msg/DataAim.msg new file mode 100644 index 0000000..3bcab37 --- /dev/null +++ b/src/rm_msgs/msg/DataAim.msg @@ -0,0 +1,9 @@ +# 视觉自瞄控制数据 + +uint8 mode # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 +float32 yaw # 目标偏航角 +float32 yaw_vel # 偏航角速度 +float32 yaw_acc # 偏航角加速度 +float32 pitch # 目标俯仰角 +float32 pitch_vel # 俯仰角速度 +float32 pitch_acc # 俯仰角加速度 diff --git a/src/rm_msgs/msg/DataMCU.msg b/src/rm_msgs/msg/DataMCU.msg new file mode 100644 index 0000000..76e4e17 --- /dev/null +++ b/src/rm_msgs/msg/DataMCU.msg @@ -0,0 +1,13 @@ +# MCU 状态数据 (MCU -> 上位机) + +uint8 mode # 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 +float32 q0 # 四元数 w +float32 q1 # 四元数 x +float32 q2 # 四元数 y +float32 q3 # 四元数 z +float32 yaw # 偏航角 +float32 yaw_vel # 偏航角速度 +float32 pitch # 俯仰角 +float32 pitch_vel # 俯仰角速度 +float32 bullet_speed # 弹速 +uint16 bullet_count # 子弹累计发送次数 diff --git a/src/rm_msgs/msg/DataRef.msg b/src/rm_msgs/msg/DataRef.msg new file mode 100644 index 0000000..32f9e64 --- /dev/null +++ b/src/rm_msgs/msg/DataRef.msg @@ -0,0 +1,5 @@ +# 裁判系统数据 + +uint16 remain_hp # 剩余血量 +uint8 game_progress # 比赛进度 +uint16 stage_remain_time # 比赛剩余时间 diff --git a/src/rm_msgs/msg/NavStatus.msg b/src/rm_msgs/msg/NavStatus.msg new file mode 100644 index 0000000..274a4b5 --- /dev/null +++ b/src/rm_msgs/msg/NavStatus.msg @@ -0,0 +1,7 @@ +# 导航状态 + +uint8 status # 0: 空闲, 1: 运行中, 2: 到达目标, 3: 失败 +float32 distance # 距离目标的距离 +float32 current_x # 当前 X 坐标 +float32 current_y # 当前 Y 坐标 +float32 current_angle # 当前角度 diff --git a/src/rm_msgs/package.xml b/src/rm_msgs/package.xml new file mode 100644 index 0000000..cdfd921 --- /dev/null +++ b/src/rm_msgs/package.xml @@ -0,0 +1,16 @@ + + + rm_msgs + 0.0.1 + Describe custom messages + biao + MIT + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/src/rm_nav/rm_nav_bringup/PCD/RMUC.pcd b/src/rm_nav/rm_nav_bringup/PCD/RMUC.pcd new file mode 100644 index 0000000..256db54 Binary files /dev/null and b/src/rm_nav/rm_nav_bringup/PCD/RMUC.pcd differ diff --git a/src/rm_nav/rm_nav_bringup/PCD/RMUL.pcd b/src/rm_nav/rm_nav_bringup/PCD/RMUL.pcd new file mode 100644 index 0000000..055222f Binary files /dev/null and b/src/rm_nav/rm_nav_bringup/PCD/RMUL.pcd differ diff --git a/src/rm_nav/rm_simple_move/src/simple_move.cpp b/src/rm_nav/rm_simple_move/src/simple_move.cpp index b82ba3a..b628440 100644 --- a/src/rm_nav/rm_simple_move/src/simple_move.cpp +++ b/src/rm_nav/rm_simple_move/src/simple_move.cpp @@ -342,18 +342,15 @@ namespace rm_simpal_move status_msg.distance = distance; if (goal_reached_) { - status_msg.status = rm_msgs::msg::NavStatus::STATUS_REACHED; - status_msg.message = "已到达目标点"; + status_msg.status = 2; // 到达目标 } else { - status_msg.status = rm_msgs::msg::NavStatus::STATUS_NAVIGATING; - status_msg.message = control_mode_ == 0 ? "PID 控制中" : "导航中"; + status_msg.status = 1; // 运行中 } nav_status_pub_->publish(status_msg); } catch (const tf2::TransformException &ex) { - status_msg.status = rm_msgs::msg::NavStatus::STATUS_FAILED; - status_msg.message = "TF 变换失败"; + status_msg.status = 3; // 失败 status_msg.distance = -1.0f; nav_status_pub_->publish(status_msg); } @@ -366,7 +363,7 @@ namespace rm_simpal_move { // 更新控制模式 uint8_t old_mode = control_mode_; - control_mode_ = msg->mode; + control_mode_ = msg->control_mode; if (old_mode != control_mode_) { // 重置 PID @@ -380,9 +377,9 @@ namespace rm_simpal_move } // 更新目标点 - goal_pose_.x = msg->target_x; - goal_pose_.y = msg->target_y; - goal_pose_.angle = msg->target_angle; + goal_pose_.x = msg->x; + goal_pose_.y = msg->y; + goal_pose_.angle = msg->angle; goal_pose_.max_speed = msg->max_speed; goal_pose_.tolerance = msg->tolerance; @@ -397,7 +394,7 @@ namespace rm_simpal_move RCLCPP_INFO(this->get_logger(), "收到新目标: mode=%s, x=%.2f, y=%.2f, angle=%.2f", control_mode_ == 0 ? "PID" : "NAV", - msg->target_x, msg->target_y, msg->target_angle); + msg->x, msg->y, msg->angle); // 如果是 NAV 模式，立即发送导航目标 if (control_mode_ == 1) { diff --git a/src/rm_serial_driver/src/rm_serial_driver.cpp b/src/rm_serial_driver/src/rm_serial_driver.cpp index 1258538..dfe4364 100644 --- a/src/rm_serial_driver/src/rm_serial_driver.cpp +++ b/src/rm_serial_driver/src/rm_serial_driver.cpp @@ -248,8 +248,8 @@ namespace rm_serial_driver } // CRC 校验（从 ID 之后开始，包含数据，不包括 CRC 本身） - if (crc16::CRC16_Verify(reinterpret_cast(&mcu) + 1, packet_size - 1)) - { + // if (crc16::CRC16_Verify(reinterpret_cast(&mcu) + 1, packet_size - 1)) + // { rm_msgs::msg::DataMCU msg; // 逐字段赋值 msg.mode = mcu.data.mode; @@ -271,14 +271,14 @@ namespace rm_serial_driver RCLCPP_DEBUG(get_logger(), "接收 MCU 数据: mode=%d, yaw=%.2f, pitch=%.2f, bullet_count=%d", mcu.data.mode, mcu.data.yaw, mcu.data.pitch, mcu.data.bullet_count); - } - else - { - crc_error_count_++; - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, - "MCU 数据 CRC 校验失败 (总计: %lu)", - crc_error_count_.load()); - } + // } + // else + // { + // crc_error_count_++; + // RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + // "MCU 数据 CRC 校验失败 (总计: %lu)", + // crc_error_count_.load()); + // } } else if (id == ID_REF) // 0xA8 - 裁判系统数据包 { diff --git a/src/rm_vision b/src/rm_vision deleted file mode 160000 index 16dfed6..0000000 --- a/src/rm_vision +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 16dfed6627a21261f628bc4a003939655c97f37b diff --git a/src/rm_vision_bringup/CMakeLists.txt b/src/rm_vision_bringup/CMakeLists.txt deleted file mode 100644 index b6c04ba..0000000 --- a/src/rm_vision_bringup/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(rm_vision_bringup) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) - -# 安装 launch 文件 -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/rm_vision_bringup/launch/calibration.launch.py b/src/rm_vision_bringup/launch/calibration.launch.py deleted file mode 100644 index 18f20df..0000000 --- a/src/rm_vision_bringup/launch/calibration.launch.py +++ /dev/null @@ -1,64 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from ament_index_python.packages import get_package_prefix -from launch.actions import ExecuteProcess - - -def generate_launch_description(): - # 声明参数 - calibration_type_arg = DeclareLaunchArgument( - 'calibration_type', - default_value='capture', - description='Calibration type: capture, calibrate_camera, calibrate_handeye, calibrate_robotworld_handeye, split_video' - ) - - config_arg = DeclareLaunchArgument( - 'config', - default_value='calibration.yaml', - description='Calibration config file name' - ) - - output_folder_arg = DeclareLaunchArgument( - 'output_folder', - default_value='assets/img_with_q', - description='Output folder for captured images (only for capture)' - ) - - # 获取参数值 - calibration_type = LaunchConfiguration('calibration_type') - config = LaunchConfiguration('config') - output_folder = LaunchConfiguration('output_folder') - - # 构建配置文件路径 - config_path = PathJoinSubstitution([ - FindPackageShare('mr_vision'), - 'configs', - config - ]) - - # 构建可执行文件路径 - pkg_prefix = get_package_prefix('mr_vision') - calibration_exe = PathJoinSubstitution([ - pkg_prefix, - 'lib', - 'mr_vision', - 'bin', - calibration_type - ]) - - # 启动标定程序 - # capture 需要额外的 output_folder 参数 - calibration_node = ExecuteProcess( - cmd=[calibration_exe, config_path, '-o', output_folder], - output='screen', - name='rm_calibration' - ) - - return LaunchDescription([ - calibration_type_arg, - config_arg, - output_folder_arg, - calibration_node - ]) diff --git a/src/rm_vision_bringup/launch/sentry_vision.launch.py b/src/rm_vision_bringup/launch/sentry_vision.launch.py deleted file mode 100644 index 726fc79..0000000 --- a/src/rm_vision_bringup/launch/sentry_vision.launch.py +++ /dev/null @@ -1,57 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from ament_index_python.packages import get_package_prefix -from launch.actions import ExecuteProcess - - -def generate_launch_description(): - """Sentry 专用视觉系统启动文件""" - - # 声明参数 - config_arg = DeclareLaunchArgument( - 'config', - default_value='sentry.yaml', - description='Sentry config file' - ) - - vision_type_arg = DeclareLaunchArgument( - 'vision_type', - default_value='standard_mpc', - description='Vision executable type for sentry' - ) - - # 获取参数值 - config = LaunchConfiguration('config') - vision_type = LaunchConfiguration('vision_type') - - # 构建配置文件路径 - config_path = PathJoinSubstitution([ - FindPackageShare('mr_vision'), - 'configs', - config - ]) - - # 构建可执行文件路径 - pkg_prefix = get_package_prefix('mr_vision') - vision_exe = PathJoinSubstitution([ - pkg_prefix, - 'lib', - 'mr_vision', - 'bin', - vision_type - ]) - - # 启动视觉节点 - vision_node = ExecuteProcess( - cmd=[vision_exe, config_path], - output='screen', - name='sentry_vision' - ) - - return LaunchDescription([ - config_arg, - vision_type_arg, - vision_node - ]) diff --git a/src/rm_vision_bringup/launch/vision.launch.py b/src/rm_vision_bringup/launch/vision.launch.py deleted file mode 100644 index 17adea9..0000000 --- a/src/rm_vision_bringup/launch/vision.launch.py +++ /dev/null @@ -1,54 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from ament_index_python.packages import get_package_prefix - - -def generate_launch_description(): - # 声明参数 - config_arg = DeclareLaunchArgument( - 'config', - default_value='standard4.yaml', - description='Vision config file name (in rm_vision/configs/)' - ) - - vision_type_arg = DeclareLaunchArgument( - 'vision_type', - default_value='standard_mpc', - description='Vision executable type: standard_mpc, balance_infantry_mpc, auto_aim_debug_mpc, auto_buff_debug_mpc' - ) - - # 获取参数值 - config = LaunchConfiguration('config') - vision_type = LaunchConfiguration('vision_type') - - # 构建配置文件路径 - config_path = PathJoinSubstitution([ - FindPackageShare('mr_vision'), - 'configs', - config - ]) - - # 构建可执行文件路径（在 lib/mr_vision/bin/ 目录下） - pkg_prefix = get_package_prefix('mr_vision') - vision_exe = PathJoinSubstitution([ - pkg_prefix, - 'lib', - 'mr_vision', - 'bin', - vision_type - ]) - - # 启动视觉节点 - vision_node = ExecuteProcess( - cmd=[vision_exe, config_path], - output='screen', - name='rm_vision' - ) - - return LaunchDescription([ - config_arg, - vision_type_arg, - vision_node - ]) diff --git a/src/rm_vision_bringup/package.xml b/src/rm_vision_bringup/package.xml deleted file mode 100644 index ac32e85..0000000 --- a/src/rm_vision_bringup/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - rm_vision_bringup - 1.0.0 - RoboMaster Vision System Launch Package - MOVE Team - MIT - - ament_cmake - - mr_vision - - ament_lint_auto - ament_lint_common - - - ament_cmake - -