diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index c9ce2e1d..c5b37764 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -119,6 +119,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual rm_common::JointPositionBinaryCommandSender* image_transmission_cmd_sender_{}; rm_common::SwitchDetectionCaller* switch_detection_srv_{}; rm_common::SwitchDetectionCaller* switch_armor_target_srv_{}; + rm_common::CalibrationQueue* chassis_calibration_; rm_common::CalibrationQueue* shooter_calibration_; rm_common::CalibrationQueue* gimbal_calibration_; diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index b3cbc881..34dfbb78 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -36,6 +36,8 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros: shooter_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); nh.getParam("gimbal_calibration", rpc_value); gimbal_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); + nh.getParam("chassis_calibration", rpc_value); + chassis_calibration_ = new rm_common::CalibrationQueue(rpc_value, nh, controller_manager_); shooter_power_on_event_.setRising(boost::bind(&ChassisGimbalShooterManual::shooterOutputOn, this)); self_inspection_event_.setRising(boost::bind(&ChassisGimbalShooterManual::selfInspectionStart, this)); game_start_event_.setRising(boost::bind(&ChassisGimbalShooterManual::gameStart, this)); @@ -71,6 +73,7 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros: void ChassisGimbalShooterManual::run() { ChassisGimbalManual::run(); + chassis_calibration_->update(ros::Time::now()); shooter_calibration_->update(ros::Time::now()); gimbal_calibration_->update(ros::Time::now()); } @@ -197,6 +200,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOff() shooter_cmd_sender_->setZero(); shooter_calibration_->stop(); gimbal_calibration_->stop(); + chassis_calibration_->stop(); use_scope_ = false; adjust_image_transmission_ = false; } @@ -206,6 +210,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOn() ChassisGimbalManual::remoteControlTurnOn(); shooter_calibration_->stopController(); gimbal_calibration_->stopController(); + chassis_calibration_->stopController(); std::string robot_color = robot_id_ >= 100 ? "blue" : "red"; switch_detection_srv_->setEnemyColor(robot_id_, robot_color); } @@ -222,6 +227,7 @@ void ChassisGimbalShooterManual::chassisOutputOn() { ChassisGimbalManual::chassisOutputOn(); chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); + chassis_calibration_->reset(); } void ChassisGimbalShooterManual::shooterOutputOn() diff --git a/src/manual_base.cpp b/src/manual_base.cpp index de10cc50..d3b1da47 100644 --- a/src/manual_base.cpp +++ b/src/manual_base.cpp @@ -79,6 +79,7 @@ void ManualBase::run() void ManualBase::checkReferee() { + chassis_power_on_event_.update(chassis_output_on_); gimbal_power_on_event_.update(gimbal_output_on_); shooter_power_on_event_.update(shooter_output_on_); referee_is_online_ = (ros::Time::now() - referee_last_get_stamp_ < ros::Duration(0.3)); @@ -110,6 +111,7 @@ void ManualBase::jointStateCallback(const sensor_msgs::JointState::ConstPtr& dat void ManualBase::actuatorStateCallback(const rm_msgs::ActuatorState::ConstPtr& data) { + updateActuatorStamp(data, chassis_mount_motor_, chassis_actuator_last_get_stamp_); updateActuatorStamp(data, gimbal_mount_motor_, gimbal_actuator_last_get_stamp_); updateActuatorStamp(data, shooter_mount_motor_, shooter_actuator_last_get_stamp_); }