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..f86c485c --- /dev/null +++ b/include/rm_manual/drone_manual.h @@ -0,0 +1,36 @@ +// +// 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_; +}; +} // namespace rm_manual 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..6344ccf7 --- /dev/null +++ b/src/drone_manual.cpp @@ -0,0 +1,92 @@ +// +// 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 }; +} + +} // namespace rm_manual 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")