From 6b46ebd4e79ab4e53c1857b71541acdb9b276d2c Mon Sep 17 00:00:00 2001 From: Zhipeng Zhong Date: Thu, 20 Nov 2025 10:19:29 +0000 Subject: [PATCH 1/2] Add drone's code. --- .../rm_manual/chassis_gimbal_shooter_manual.h | 3 +- include/rm_manual/drone_manual.h | 37 ++++++++ src/chassis_gimbal_shooter_manual.cpp | 2 + src/drone_manual.cpp | 86 +++++++++++++++++++ src/main.cpp | 5 +- 5 files changed, 131 insertions(+), 2 deletions(-) create mode 100644 include/rm_manual/drone_manual.h create mode 100644 src/drone_manual.cpp diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index c85d0996..efa30bad 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -105,12 +105,12 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP); } void ctrlVPress(); - void ctrlBPress(); void ctrlRPress(); void ctrlZPress(); void ctrlXPress(); virtual void ctrlRRelease(); virtual void ctrlQPress(); + virtual void ctrlBPress(); InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_, r_event_, v_event_, z_event_, ctrl_f_event_, ctrl_v_event_, ctrl_b_event_, ctrl_q_event_, ctrl_r_event_, @@ -120,6 +120,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual rm_common::JointPositionBinaryCommandSender* scope_cmd_sender_{}; rm_common::JointPositionBinaryCommandSender* image_transmission_cmd_sender_{}; rm_common::SwitchDetectionCaller* switch_detection_srv_{}; + rm_common::SwitchDetectionCaller* switch_detection_left_srv_{}; rm_common::SwitchDetectionCaller* switch_armor_target_srv_{}; rm_common::CalibrationQueue* chassis_calibration_; rm_common::CalibrationQueue* shooter_calibration_; diff --git a/include/rm_manual/drone_manual.h b/include/rm_manual/drone_manual.h new file mode 100644 index 00000000..94d4abc4 --- /dev/null +++ b/include/rm_manual/drone_manual.h @@ -0,0 +1,37 @@ +// +// Created by cilotta on 25-7-16. +// +#pragma once + +#include "rm_manual/chassis_gimbal_shooter_manual.h" +#include "rm_msgs/PriorityArray.h" + +namespace rm_manual +{ +class DroneManual : public ChassisGimbalShooterManual +{ +public: + DroneManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee); + +protected: + void ePress() override; + void eRelease() override; + void rPress() override; + void gPress() override; + void vPress() override; + void remoteControlTurnOn() override; + + void ePressing(); + void eReleasing(); + void qPressing(); + void qReleasing(); + + void ctrlBPress() override; + void pubAimPriority(); + + rm_common::SwitchDetectionCaller* switch_detection_left_srv_{}; + ros::Publisher aim_priority_pub_; + rm_msgs::PriorityArray aim_priority_array_; +}; +} + diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 913b7f10..a6d2b723 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -29,7 +29,9 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros: } ros::NodeHandle detection_switch_nh(nh, "detection_switch"); + ros::NodeHandle detection_switch_left_nh(nh, "detection_switch_left"); switch_detection_srv_ = new rm_common::SwitchDetectionCaller(detection_switch_nh); + switch_detection_left_srv_ = new rm_common::SwitchDetectionCaller(detection_switch_left_nh); ros::NodeHandle armor_target_switch_nh(nh, "armor_target_switch"); switch_armor_target_srv_ = new rm_common::SwitchDetectionCaller(armor_target_switch_nh); XmlRpc::XmlRpcValue rpc_value; diff --git a/src/drone_manual.cpp b/src/drone_manual.cpp new file mode 100644 index 00000000..23741f66 --- /dev/null +++ b/src/drone_manual.cpp @@ -0,0 +1,86 @@ +// +// Created by cilotta on 25-7-16. +// + +#include "rm_manual/drone_manual.h" + +namespace rm_manual +{ + DroneManual::DroneManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) +: ChassisGimbalShooterManual(nh, nh_referee) +{ + ros::NodeHandle detection_switch_left_nh(nh, "detection_switch_left"); + switch_detection_left_srv_ = new rm_common::SwitchDetectionCaller(detection_switch_left_nh); + e_event_.setActive(boost::bind(&DroneManual::ePressing, this), + boost::bind(&DroneManual::eReleasing, this)); + q_event_.setActive(boost::bind(&DroneManual::qPressing, this), + boost::bind(&DroneManual::qReleasing, this)); + aim_priority_pub_ = nh.advertise("/armor_processor_right/priority/priority_arr", 1); +} + +void DroneManual::ePress(){ePressing();} +void DroneManual::eRelease(){eReleasing();} + +void DroneManual::ePressing() + { + //switch_armor_target_srv_->setArmorTargetType(rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE); + //switch_armor_target_srv_->callService(); + //shooter_cmd_sender_->setArmorType(switch_armor_target_srv_->getArmorTarget()); + camera_switch_cmd_sender_->switchCameraLeft(); + }; + +void DroneManual::eReleasing() +{ + //switch_armor_target_srv_->setArmorTargetType(rm_msgs::StatusChangeRequest::ARMOR_ALL); + //switch_armor_target_srv_->callService(); + //shooter_cmd_sender_->setArmorType(switch_armor_target_srv_->getArmorTarget()); + camera_switch_cmd_sender_->switchCameraRight(); +} + +void DroneManual::qPressing() +{ + aim_priority_array_.rank_arr = {0,0,0,0,0,0,0,1}; + pubAimPriority(); +} + +void DroneManual::qReleasing() +{ + aim_priority_array_.rank_arr = {1,1,1,1,1,1,1,1}; + pubAimPriority(); +} + +void DroneManual::pubAimPriority() +{ + aim_priority_pub_.publish(aim_priority_array_); +} + +void DroneManual::rPress(){ + if (camera_switch_cmd_sender_) + camera_switch_cmd_sender_->switchCameraLeft(); + ChassisGimbalShooterManual::rPress(); +}; + +void DroneManual::ctrlBPress(){ + ChassisGimbalShooterManual::ctrlBPress(); + switch_detection_left_srv_->switchEnemyColor(); + switch_detection_left_srv_->callService(); +} + +void DroneManual::gPress() +{ + ChassisGimbalShooterManual::gPress(); +} + +void DroneManual::vPress() +{ + ChassisGimbalShooterManual::vPress(); +} + +void DroneManual::remoteControlTurnOn(){ + ChassisGimbalShooterManual::remoteControlTurnOn(); + std::string robot_color = robot_id_ >= 100 ? "blue" : "red"; + switch_detection_left_srv_->setEnemyColor(robot_id_,robot_color); + aim_priority_array_.rank_arr = {1,1,1,1,1,1,1,1}; +} + +} diff --git a/src/main.cpp b/src/main.cpp index 5e3bb26a..7b6abad2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,6 +8,7 @@ #include "rm_manual/engineer2_manual.h" #include "rm_manual/dart_manual.h" #include "rm_manual/wheeled_balance_manual.h" +#include "rm_manual/drone_manual.h" #include "rm_manual/legged_wheel_balance_manual.h" int main(int argc, char** argv) @@ -20,8 +21,10 @@ int main(int argc, char** argv) robot = getParam(nh, "robot_type", (std::string) "error"); if (robot == "standard") manual_control = new rm_manual::ChassisGimbalShooterCoverManual(nh, nh_referee); - else if ((robot == "hero") || (robot == "drone")) + else if (robot == "hero") manual_control = new rm_manual::ChassisGimbalShooterManual(nh, nh_referee); + else if (robot == "drone") + manual_control = new rm_manual::DroneManual(nh, nh_referee); else if (robot == "engineer") manual_control = new rm_manual::EngineerManual(nh, nh_referee); else if (robot == "engineer2") From 38983ecff7b0bda57a00949ea774934e72921460 Mon Sep 17 00:00:00 2001 From: Zhipeng Zhong Date: Wed, 10 Dec 2025 07:19:44 +0000 Subject: [PATCH 2/2] Fix code format. --- include/rm_manual/drone_manual.h | 3 +- src/drone_manual.cpp | 62 +++++++++++++++++--------------- 2 files changed, 35 insertions(+), 30 deletions(-) diff --git a/include/rm_manual/drone_manual.h b/include/rm_manual/drone_manual.h index 94d4abc4..f86c485c 100644 --- a/include/rm_manual/drone_manual.h +++ b/include/rm_manual/drone_manual.h @@ -33,5 +33,4 @@ class DroneManual : public ChassisGimbalShooterManual ros::Publisher aim_priority_pub_; rm_msgs::PriorityArray aim_priority_array_; }; -} - +} // namespace rm_manual diff --git a/src/drone_manual.cpp b/src/drone_manual.cpp index 23741f66..6344ccf7 100644 --- a/src/drone_manual.cpp +++ b/src/drone_manual.cpp @@ -6,46 +6,49 @@ namespace rm_manual { - DroneManual::DroneManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) -: ChassisGimbalShooterManual(nh, nh_referee) +DroneManual::DroneManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : ChassisGimbalShooterManual(nh, nh_referee) { - ros::NodeHandle detection_switch_left_nh(nh, "detection_switch_left"); - switch_detection_left_srv_ = new rm_common::SwitchDetectionCaller(detection_switch_left_nh); - e_event_.setActive(boost::bind(&DroneManual::ePressing, this), - boost::bind(&DroneManual::eReleasing, this)); - q_event_.setActive(boost::bind(&DroneManual::qPressing, this), - boost::bind(&DroneManual::qReleasing, this)); - aim_priority_pub_ = nh.advertise("/armor_processor_right/priority/priority_arr", 1); + ros::NodeHandle detection_switch_left_nh(nh, "detection_switch_left"); + switch_detection_left_srv_ = new rm_common::SwitchDetectionCaller(detection_switch_left_nh); + e_event_.setActive(boost::bind(&DroneManual::ePressing, this), boost::bind(&DroneManual::eReleasing, this)); + q_event_.setActive(boost::bind(&DroneManual::qPressing, this), boost::bind(&DroneManual::qReleasing, this)); + aim_priority_pub_ = nh.advertise("/armor_processor_right/priority/priority_arr", 1); } -void DroneManual::ePress(){ePressing();} -void DroneManual::eRelease(){eReleasing();} +void DroneManual::ePress() +{ + ePressing(); +} +void DroneManual::eRelease() +{ + eReleasing(); +} void DroneManual::ePressing() - { - //switch_armor_target_srv_->setArmorTargetType(rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE); - //switch_armor_target_srv_->callService(); - //shooter_cmd_sender_->setArmorType(switch_armor_target_srv_->getArmorTarget()); - camera_switch_cmd_sender_->switchCameraLeft(); - }; +{ + // switch_armor_target_srv_->setArmorTargetType(rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE); + // switch_armor_target_srv_->callService(); + // shooter_cmd_sender_->setArmorType(switch_armor_target_srv_->getArmorTarget()); + camera_switch_cmd_sender_->switchCameraLeft(); +}; void DroneManual::eReleasing() { - //switch_armor_target_srv_->setArmorTargetType(rm_msgs::StatusChangeRequest::ARMOR_ALL); - //switch_armor_target_srv_->callService(); - //shooter_cmd_sender_->setArmorType(switch_armor_target_srv_->getArmorTarget()); + // switch_armor_target_srv_->setArmorTargetType(rm_msgs::StatusChangeRequest::ARMOR_ALL); + // switch_armor_target_srv_->callService(); + // shooter_cmd_sender_->setArmorType(switch_armor_target_srv_->getArmorTarget()); camera_switch_cmd_sender_->switchCameraRight(); } void DroneManual::qPressing() { - aim_priority_array_.rank_arr = {0,0,0,0,0,0,0,1}; + aim_priority_array_.rank_arr = { 0, 0, 0, 0, 0, 0, 0, 1 }; pubAimPriority(); } void DroneManual::qReleasing() { - aim_priority_array_.rank_arr = {1,1,1,1,1,1,1,1}; + aim_priority_array_.rank_arr = { 1, 1, 1, 1, 1, 1, 1, 1 }; pubAimPriority(); } @@ -54,13 +57,15 @@ void DroneManual::pubAimPriority() aim_priority_pub_.publish(aim_priority_array_); } -void DroneManual::rPress(){ +void DroneManual::rPress() +{ if (camera_switch_cmd_sender_) camera_switch_cmd_sender_->switchCameraLeft(); ChassisGimbalShooterManual::rPress(); }; -void DroneManual::ctrlBPress(){ +void DroneManual::ctrlBPress() +{ ChassisGimbalShooterManual::ctrlBPress(); switch_detection_left_srv_->switchEnemyColor(); switch_detection_left_srv_->callService(); @@ -76,11 +81,12 @@ void DroneManual::vPress() ChassisGimbalShooterManual::vPress(); } -void DroneManual::remoteControlTurnOn(){ +void DroneManual::remoteControlTurnOn() +{ ChassisGimbalShooterManual::remoteControlTurnOn(); std::string robot_color = robot_id_ >= 100 ? "blue" : "red"; - switch_detection_left_srv_->setEnemyColor(robot_id_,robot_color); - aim_priority_array_.rank_arr = {1,1,1,1,1,1,1,1}; + switch_detection_left_srv_->setEnemyColor(robot_id_, robot_color); + aim_priority_array_.rank_arr = { 1, 1, 1, 1, 1, 1, 1, 1 }; } -} +} // namespace rm_manual