diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index f1c13625..42c3e5ca 100755 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -527,4 +527,4 @@ void ChassisBase::capacityCallback(const rm_msgs::PowerManagementSampleAnd } } -} // namespace rm_chassis_controllers \ No newline at end of file +} // namespace rm_chassis_controllers diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98e8736b..9a3a0e1c 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -14,6 +14,7 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp + std_msgs rm_common controller_interface effort_controllers @@ -45,6 +46,7 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp + std_msgs rm_common effort_controllers tf2_eigen @@ -65,7 +67,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 3cf38627..0abda770 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -10,22 +10,26 @@ gen.add("resistance_coff_qd_15", double_t, 0, "Air resistance divided by mass of gen.add("resistance_coff_qd_16", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) +gen.add("resistance_coff_qd_800", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) +gen.add("outpost_delay", double_t, 0, "Delay of bullet firing of outpost", 0.0, 0, 0.5) gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5) gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) gen.add("switch_angle_offset", double_t, 0, "Switch angle offset", 0.0, -20.0, 20) -gen.add("switch_duration_scale",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("switch_duration_rate",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("switch_duration_offset",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) -gen.add("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4) +gen.add("outpost_switch_angle_offset", double_t, 0,"Switch angle offset", 0.0, -20.0, 20) +gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99) +gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of switch", 0, 1, 99) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) +gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -400.0, 400.0) +gen.add("track_rotate_outpost_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0) +gen.add("traject_k_effort",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) +gen.add("traject_k_vel",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 5f614c33..6c6a3107 100644 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -13,5 +13,6 @@ gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0, gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_d_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) +gen.add("moment_of_inertia_", double_t, 0, "Moment of inertia of gimbal", 0.0, 0, 999.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 163f8475..d0b3cb32 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -57,15 +57,15 @@ namespace rm_gimbal_controllers { struct Config { - double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale, - switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, track_rotate_target_delay, - track_move_target_delay; - int min_fit_switch_count; - int max_selected_armor_; + double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, + resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, outpost_delay, center_delay, + max_switch_angle, switch_angle_offset, outpost_switch_angle_offset, min_shoot_beforehand_vel, + track_rotate_target_delay, track_move_target_delay, yaw_max_acc, track_rotate_outpost_delay; + int min_fit_switch_count, traject_start_fit_; double traject_ahead_; int clean_shoot_num_; double end_pos_offset; + double traject_k_effort, traject_k_vel_; }; struct TrajectoryFunctionCoefficients { @@ -82,22 +82,27 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel); + double r1, double r2, double dz, double armors_num, double start_vel, double track_id); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, - double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); - double getResistanceCoefficient(double bullet_speed) const; + double r2, double dz, double armors_num, double yaw_real, double pitch_real, + double bullet_speed); + double getResistanceCoefficient(double target_distance) const; double getYaw() const { - return output_yaw_; + return output_yaw_[0]; } double getPitch() const { - return -output_pitch_; + return -output_pitch_[0]; } double getTrajectYaw() const { return traject_output_yaw_; } + double getTrajectVel() const + { + return traject_vel_; + } bool getUsingtraject() const { return using_traject_; @@ -110,26 +115,32 @@ class BulletSolver { return traject_effort_ff_; } - bool getTrackTarget() + bool getTrackTarget() const { return track_target_; } - double getGimbalSwitchDuration(double v_yaw); + void CleanTrackCount() + { + track_count_ = 0; + } + double getFlyTime() + { + return fly_time_[0]; + } void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num); - void judgeShootBeforehand(const ros::Time& time, double v_yaw); + double r1, double r2, double dz, double armors_num); + uint8_t judgeShootBeforehand(double v_yaw, int id); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); - double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_); + double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw); void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg); ~BulletSolver() = default; private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> shoot_beforehand_cmd_pub_; std::shared_ptr> fly_time_pub_; std::shared_ptr> bullet_solver_pub; ros::Subscriber identified_target_change_sub_; @@ -138,46 +149,56 @@ class BulletSolver realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; - double max_track_target_vel_; - double output_yaw_{}, output_pitch_{}, traject_output_yaw_{}; + double yaw_[150], pos_x[150], pos_y[150]; + double max_track_target_vel_{}; + double output_yaw_[150], output_pitch_[150], traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; - double fly_time_; - double switch_hysteresis_; + double fly_time_[150]; + double switch_hysteresis_{}; double last_yaw_{}, filtered_yaw_{}; double gimbal_switch_duration_{}; - double yaw_subtract_; - double switch_armor_angle; + double yaw_subtract_{}; + double switch_armor_angle{}; double filtered_v_yaw_{}; - double switchtime; - double traject_effort_ff_; - double traject_switch_time_; - double switche_time_yaw_; - double traject_max_acc_; - int shoot_num_ = 0; - int shoot_beforehand_cmd_{}; - int count_; - int traject_count_; - int ban_shoot_count_ = 0; - int selected_armor_ = 0; + double switchtime{}; + double traject_effort_ff_{}; + double traject_switch_time_{}; + double switche_time_yaw_{}; + double traject_max_acc_{}; + double last_output_yaw_{}; + double traject_vel_{}; + double r_traject_{}; + double intital_yaw_{}; - bool track_target_ = true; + int shoot_num_{}; + int shoot_beforehand_cmd_{}; + int count_[150]{}; + int next_count_[150]{}; + int ban_shoot_count_{}; + int selected_armor_[150] = {}; + int last_selected_armor_ = {}; + bool track_target_ = false; + int track_count_{}; bool identified_target_change_ = true; - bool is_in_delay_before_switch_{}; bool dynamic_reconfig_initialized_{}; - bool change_armor = false; - bool using_traject_; - bool last_shoot_state_; + bool using_traject_{}; + bool last_shoot_state_{}; + bool is_aheading_two_[150]{}; + bool start_traject_{}; + double filtered_vel_des{}; geometry_msgs::Point after_traject_output_yaw_{}; - geometry_msgs::Point target_pos_{}; + geometry_msgs::Point target_pos_[150]{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; - ros::Time start_using_traject_time; - ros::Time ban_shoot_time_; + ros::Time start_using_traject_time{}; + ros::Time ban_shoot_time_{}; + ros::Time last_output_time_{}; + ros::Time ban_shoot_start_time_{}; - TrajectoryFunctionCoefficients trajectory_function_coefficients; - TrajectoryLimitParams stauts_limit_; + TrajectoryFunctionCoefficients trajectory_function_coefficients{}; + TrajectoryLimitParams stauts_limit_{}; - mutable std::mutex heat_mutex_; + mutable std::mutex heat_mutex_{}; }; } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h new file mode 100644 index 00000000..85dccb45 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * BSD 3-Clause License + ******************************************************************************/ + +#pragma once + +#include + +namespace rm_gimbal_controllers +{ +class DualYawController : public Controller +{ +public: + DualYawController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + +protected: + bool shouldInitializeController(const std::string& name, const urdf::JointConstSharedPtr& joint_urdf, + int axis) const override; + bool shouldApplyJointLimit(int axis) const override; + std::string getBaseFrameID(const std::unordered_map& joint_urdfs) override; + void updateYawJoint(const ros::Time& time, const ros::Duration& period, const geometry_msgs::Vector3& angular_vel, + const double pos_real[3], const double pos_des[3], const double pos_des_temp[3], + const double vel_des[3], const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]) override; + +private: + bool initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh); + bool getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point); + void publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error); + + urdf::JointConstSharedPtr base_yaw_joint_; + std::unique_ptr base_yaw_ctrl_; + std::unique_ptr base_yaw_pid_pos_; + std::unique_ptr> base_yaw_pos_state_pub_; +}; +} // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 50c3eff2..48a9fcfc 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -66,13 +70,15 @@ namespace rm_gimbal_controllers struct GimbalConfig { double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; - double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, chassis_comp_d_{}; + double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, + chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double moment_of_inertia_{}; }; class ChassisVel { public: - explicit ChassisVel(const ros::NodeHandle& nh) + ChassisVel(const ros::NodeHandle& nh) { double num_data; nh.param("num_data", num_data, 20.0); @@ -141,26 +147,48 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); + virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); + virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]); + virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], + const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]); + void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, + double k_v, const double vel_des[3], const double angle_error[3], + const double traject_angle_error[3]); void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time); + void track(const ros::Time& time, const ros::Duration& period); + void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); + bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); + double gravityFeedForward(const ros::Time& time); void updateChassisVel(); void updateBallisticSolution(const ros::Time& time); - double gravityFeedForward(const ros::Time& time); double updateCompensation(double chassis_vel_angular_z); + void publishShootBeforehand(const ros::Time& time, uint8_t cmd); void commandCB(const rm_msgs::GimbalCmdConstPtr& msg); void trackCB(const rm_msgs::TrackDataConstPtr& msg); void ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg); + void mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg); void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t); - std::string getGimbalFrameID(std::unordered_map joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); + bool TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res); + void publishTargetIsArmor(); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; @@ -173,22 +201,38 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; std::shared_ptr ballistic_solver_; + double gimbal_real_z_vel_{}; + enum class TrackSolverType + { + BULLET_SOLVER, + EXTERNAL_MPC_AIM + }; + TrackSolverType track_solver_type_{ TrackSolverType::BULLET_SOLVER }; + double external_aim_timeout_{ 0.1 }; + bool external_aim_active_{ false }; // ROS Interface ros::Time last_publish_time_{}; ros::Time last_ballistic_publish_time_{}; + ros::Time last_track_time_{}; std::unordered_map>> pos_state_pub_; std::shared_ptr> error_pub_; std::shared_ptr> ballistic_solution_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; ros::Subscriber ballistic_solver_request_sub_; + ros::Subscriber mpc_gimbal_aim_sub_; + ros::ServiceServer TrackSolver_type_srv_; + ros::Publisher target_is_armor_pub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; realtime_tools::RealtimeBuffer ballistic_track_rt_buffer_; + realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; + rm_msgs::MPCGimbalAimCmd gimbal_aim_; std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_; double publish_rate_{}; bool state_changed_{}; @@ -221,9 +265,12 @@ class Controller : public controller_interface::MultiInterfaceControllercatkin roscpp + std_msgs rm_common diff --git a/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml b/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml index 8f436457..b12da6bd 100644 --- a/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml +++ b/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml @@ -8,4 +8,11 @@ + + + The DualYawController controls base_yaw, yaw and pitch from one GimbalCmd source. + + + diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 06f330f3..a604342d 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -40,6 +40,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -51,22 +52,25 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .resistance_coff_qd_16 = getParam(controller_nh, "resistance_coff_qd_16", 0.), .resistance_coff_qd_18 = getParam(controller_nh, "resistance_coff_qd_18", 0.), .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.), + .resistance_coff_qd_800 = getParam(controller_nh, "resistance_coff_qd_800", 0.001), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), + .outpost_delay = getParam(controller_nh, "outpost_delay", 0.), .center_delay = getParam(controller_nh, "center_delay", 0.0), - .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.0), - .switch_duration_scale = getParam(controller_nh, "switch_duration_scale", 0.), - .switch_duration_rate = getParam(controller_nh, "switch_duration_rate", 0.), - .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.1), - .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 3.0), + .outpost_switch_angle_offset = getParam(controller_nh, "outpost_switch_angle_offset", 0.0), + .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 2.5), .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), + .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), + .track_rotate_outpost_delay = getParam(controller_nh, "track_rotate_outpost_delay", 0.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), - .max_selected_armor_ = getParam(controller_nh, "max_selecteed_armor", 3), + .traject_start_fit_ = getParam(controller_nh, "traject_start_fit_", 45), .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1), .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.), + .traject_k_effort = getParam(controller_nh, "traject_k_effort", 0.0), + .traject_k_vel_ = getParam(controller_nh, "traject_k_vel", 0.65), }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); @@ -96,8 +100,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - shoot_beforehand_cmd_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); bullet_solver_pub.reset( new realtime_tools::RealtimePublisher(controller_nh, "bullet_solver_data", 10)); @@ -107,352 +109,385 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) &BulletSolver::heatCB, this); } -double BulletSolver::getResistanceCoefficient(double bullet_speed) const +double BulletSolver::getResistanceCoefficient(double target_distance) const { - // bullet_speed have 5 value:10,15,16,18,30 double resistance_coff; - if (bullet_speed < 12.5) - resistance_coff = config_.resistance_coff_qd_10; - else if (bullet_speed < 15.5) - resistance_coff = config_.resistance_coff_qd_15; - else if (bullet_speed < 17) - resistance_coff = config_.resistance_coff_qd_16; - else if (bullet_speed < 24) - resistance_coff = config_.resistance_coff_qd_18; - else - resistance_coff = config_.resistance_coff_qd_30; + // hero + resistance_coff = config_.resistance_coff_qd_800; return resistance_coff; } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel) + double v_yaw, double r1, double r2, double dz, double armors_num, double start_vel, + double track_id) { + while (yaw > M_PI / 2) + yaw -= M_PI; + while (yaw < -M_PI / 2) + yaw += M_PI; + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; - resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; if (abs(yaw - last_yaw_) > 1.) filtered_yaw_ = yaw; else if (last_yaw_ != yaw) - { filtered_yaw_ = filtered_yaw_ + (yaw - filtered_yaw_) * (0.001 / (0.01 + 0.001)); - } last_yaw_ = yaw; filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05; - double temp_z = pos.z; - double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); - double output_yaw_central = std::atan2(pos.y, pos.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); - double rough_fly_time = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double r = r1; - double z = pos.z; - if (track_target_) - yaw += filtered_v_yaw_ * config_.track_rotate_target_delay; - pos.x += vel.x * config_.track_move_target_delay; - pos.y += vel.y * config_.track_move_target_delay; - if (track_target_) + if (track_count_ <= 300) { - if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) - track_target_ = false; + track_target_ = false; + track_count_++; } - else + if (track_count_ > 300) { - if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) + if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) + track_target_ = false; + else if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) track_target_ = true; - } - switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; - traject_switch_time_ = 1.321 * exp(-0.289 * abs(filtered_v_yaw_ + 1.0)) * config_.traject_ahead_; - // traject_switch_time_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_; - // yaw_subtract_ = filtered_yaw_ - output_yaw_central; - yaw_subtract_ = filtered_yaw_ - output_yaw_central; - while (yaw_subtract_ > M_PI) - yaw_subtract_ -= 2 * M_PI; - while (yaw_subtract_ < -M_PI) - yaw_subtract_ += 2 * M_PI; - double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; - if (((after_fly_yaw_subtract_ < switch_armor_angle - 0.3 && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ > switch_armor_angle + 0.3 && filtered_v_yaw_ < -1.)) && - track_target_) - { - selected_armor_ = 0; - } - else if (!track_target_) - { - selected_armor_ = 0; + if (sqrt(pow(pos.x, 2) + pow(pos.y, 2)) < 1.2) + track_target_ = false; } - if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.)) + if (identified_target_change_) { - count_++; - if (change_armor) + for (int i = 0; i < 150; i++) { - count_ = 0; - change_armor = false; - switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; + count_[i] = 0; + next_count_[i] = 0; } - if (count_ >= config_.min_fit_switch_count) + identified_target_change_ = false; + } + for (int i = 0; i < 150; i++) + { + double temp_z = pos.z; + if (track_target_) { - selected_armor_ = v_yaw > 0. ? -1 : 1; - r = armors_num == 4 ? r2 : r1; - z = pos.z + dz; - if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) - { - selected_armor_ = -2; - } - else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < switch_armor_angle) && v_yaw < 0.) + if (track_id == 6) { - selected_armor_ = 2; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_outpost_delay + i * 0.001); } - if (count_ == config_.min_fit_switch_count) + else { - switch_armor_time_ = ros::Time::now(); - traject_count_ = 0; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); } } - } - if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 2. && - count_ > 2) - { - traject_count_++; - if (traject_count_ == 2) + else + yaw_[i] = yaw; + pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.001); + pos_y[i] = pos.y + vel.y * (config_.track_move_target_delay + i * 0.001); + double target_rho = std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2)); + resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; + double output_yaw_central = std::atan2(pos_y[i], pos_x[i]); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2))); + double rough_fly_time = + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + if (bullet_speed == 0.0) { - using_traject_ = true; - start_using_traject_time = ros::Time::now(); + ROS_ERROR("solve bullet speed is zero"); + return false; } - } - double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 1.57; - double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_central; - while (aim_yaw_subtract > M_PI) - aim_yaw_subtract -= 2 * M_PI; - while (aim_yaw_subtract < -M_PI) - aim_yaw_subtract += 2 * M_PI; - is_in_delay_before_switch_ = - ((((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) > switch_armor_angle) && filtered_v_yaw_ > 1.) || - (((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) < -switch_armor_angle) && filtered_v_yaw_ < -1.)) && - track_target_; - int count{}; - double error = 999; - if (track_target_) - { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 1.57); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.57); - } - else - { - target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); - target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); - if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.5) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.5)) + double r = r1; + double z = pos.z; + + switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; + if (track_id == 6) { - selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1; + switch_armor_angle = acos(r1 / target_rho) - config_.outpost_switch_angle_offset; } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.5))) + yaw_subtract_ = yaw_[i] - output_yaw_central; + // while (yaw_subtract_ > M_PI) + // yaw_subtract_ -= 2 * M_PI; + // while (yaw_subtract_ < -M_PI) + // yaw_subtract_ += 2 * M_PI; + double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; + if (abs(v_yaw) < 2.) + selected_armor_[i] = 0; + else if ((after_fly_yaw_subtract_ < switch_armor_angle - 0.4 && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ > -switch_armor_angle + 0.4 && filtered_v_yaw_ < -2.)) + selected_armor_[i] = 0; + + if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -2.)) { - selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; + count_[i]++; + if (count_[i] >= config_.min_fit_switch_count) + { + selected_armor_[i] = v_yaw > 0. ? -1 : 1; + r = armors_num == 4 ? r2 : r1; + z = pos.z + dz; + if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) + { + selected_armor_[i] = -2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < -switch_armor_angle) && v_yaw < 0.) + { + selected_armor_[i] = 2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + if (count_[i] == config_.min_fit_switch_count) + { + if (is_aheading_two_[i]) + { + is_aheading_two_[i] = false; + } + else + { + switch_armor_time_ = ros::Time::now(); + } + } + } } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.5))) + int count{}; + double error = 999; + if (track_target_) { - selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; + target_pos_[i].x = pos_x[i] - r * cos(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] - r * sin(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); } - if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || - (selected_armor_ < 0 && selected_armor_ < -config_.max_selected_armor_)) + else { - selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; + target_pos_[i].x = pos_x[i] - r * cos(atan2(pos_y[i], pos_x[i])); + target_pos_[i].y = pos_y[i] - r * sin(atan2(pos_y[i], pos_x[i])); + selected_armor_[i] = 0; + if ((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -0.7)) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -1 : 1; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 1 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 1 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -2 : 2; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 2 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 2 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -3 : 3; + } + + if (selected_armor_[i] % 2 == 0) + { + r = r1; + z = pos.z; + } + else + { + r = r2; + z = pos.z + dz; + } } - if (selected_armor_ % 2 == 0) + target_pos_[i].z = z; + while (error >= 0.001) { - r = r1; - z = pos.z; + output_yaw_[i] = std::atan2(target_pos_[i].y, target_pos_[i].x); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2))); + target_rho = std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2)); + fly_time_[i] = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + double real_z = (bullet_speed_ * std::sin(output_pitch_[i]) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time_[i])) / resistance_coff_ - + config_.g * fly_time_[i] / resistance_coff_; + if (track_target_) + { + target_pos_[i].x = pos_x[i] + vel.x * fly_time_[i] - + r * cos(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] + vel.y * fly_time_[i] - + r * sin(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + } + else + { + double target_pos_after_fly_time[2]; + target_pos_after_fly_time[0] = pos_x[i] + vel.x * fly_time_[i]; + target_pos_after_fly_time[1] = pos_y[i] + vel.y * fly_time_[i]; + target_pos_[i].x = + target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + target_pos_[i].y = + target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + } + target_pos_[i].z = z + vel.z * fly_time_[i]; + + double target_yaw = std::atan2(target_pos_[i].y, target_pos_[i].x); + double error_theta = target_yaw - output_yaw_[i]; + double error_z = target_pos_[i].z - real_z; + temp_z += error_z; + error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); + count++; + if (count >= 20 || std::isnan(error)) + return false; } } - target_pos_.z = z; - double r_trajcet_; - if (selected_armor_ == 1 || selected_armor_ == -1) + if (fly_time_pub_->trylock()) { - r_trajcet_ = r1; + fly_time_pub_->msg_.data = fly_time_[0]; + fly_time_pub_->unlockAndPublish(); + } + if (selected_armor_[0] == 1 || selected_armor_[0] == -1 || selected_armor_[0] == 3 || selected_armor_[0] == -3) + { + r_traject_ = r1; } else { - r_trajcet_ = r2; + r_traject_ = r2; } - - if (!using_traject_) + for (int i = 1; i <= config_.traject_start_fit_; i++) { - switchtime = 0.08; - - if (filtered_v_yaw_ > 0) - { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - } - else + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 4.5) { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); + start_traject_ = true; + using_traject_ = true; + start_using_traject_time = ros::Time::now(); + break; } - stauts_limit_.start_pos = output_yaw_; - stauts_limit_.start_vel = start_vel; - stauts_limit_.end_pos = - std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; - stauts_limit_.end_vel = stauts_limit_.start_vel; + } - if (switchtime > 0.2) + if (start_traject_ && using_traject_) + { + traject_max_acc_ = 400.; + switchtime = 0.05; + while (abs(traject_max_acc_) > config_.yaw_max_acc) { - using_traject_ = false; - } - trajectory_function_coefficients.a0 = stauts_limit_.start_pos; - trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - - Eigen::Matrix2d A; - A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); - Eigen::Vector2d B; - B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), - stauts_limit_.end_vel - stauts_limit_.start_vel; + switchtime += 0.005; + if (filtered_v_yaw_ > 0) + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + } + else + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + } + stauts_limit_.start_pos = output_yaw_[0]; + stauts_limit_.start_vel = start_vel; + stauts_limit_.end_pos = + std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; + stauts_limit_.end_pos = + stauts_limit_.start_pos + angles::shortest_angular_distance(stauts_limit_.start_pos, stauts_limit_.end_pos); + stauts_limit_.end_vel = stauts_limit_.start_vel; - Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); + if (switchtime > 0.15) + { + break; + } + trajectory_function_coefficients.a0 = stauts_limit_.start_pos; + trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - trajectory_function_coefficients.a2 = X(0); - trajectory_function_coefficients.a3 = X(1); - traject_max_acc_ = 2 * trajectory_function_coefficients.a2; - } + Eigen::Matrix2d A; + A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); + Eigen::Vector2d B; + B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), + stauts_limit_.end_vel - stauts_limit_.start_vel; - while (error >= 0.001) - { - output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2))); - target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); - fly_time_ = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; + Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); - if (track_target_) - { - target_pos_.x = - pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = - pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); + trajectory_function_coefficients.a2 = X(0); + trajectory_function_coefficients.a3 = X(1); + traject_max_acc_ = abs(2 * trajectory_function_coefficients.a2); } - else - { - double target_pos_after_fly_time[2]; - target_pos_after_fly_time[0] = pos.x + vel.x * fly_time_; - target_pos_after_fly_time[1] = pos.y + vel.y * fly_time_; - target_pos_.x = - target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - target_pos_.y = - target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - } - target_pos_.z = z + vel.z * fly_time_; - - double target_yaw = std::atan2(target_pos_.y, target_pos_.x); - double error_theta = target_yaw - output_yaw_; - double error_z = target_pos_.z - real_z; - temp_z += error_z; - error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); - count++; - - if (count >= 20 || std::isnan(error)) - return false; - } - if (fly_time_pub_->trylock()) - { - fly_time_pub_->msg_.data = fly_time_; - fly_time_pub_->unlockAndPublish(); + start_traject_ = false; } if (using_traject_) { ros::Time temp = ros::Time::now(); - traject_output_yaw_ = planningPoint(temp, start_using_traject_time); + traject_output_yaw_ = planningPoint(temp, start_using_traject_time, v_yaw); + while (traject_output_yaw_ > M_PI) + traject_output_yaw_ -= 2 * M_PI; + while (traject_output_yaw_ < -M_PI) + traject_output_yaw_ += 2 * M_PI; if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) { using_traject_ = false; } } - else + if (!using_traject_ || !track_target_) { - traject_output_yaw_ = output_yaw_; + traject_output_yaw_ = output_yaw_[0]; } + double vel_des = (traject_output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + filtered_vel_des = filtered_vel_des + (vel_des - filtered_vel_des) * (0.001 / (0.01 + 0.001)); + last_output_time_ = ros::Time::now(); + last_output_yaw_ = traject_output_yaw_; if (bullet_solver_pub->trylock()) { - bullet_solver_pub->msg_.selected_armor_ = selected_armor_; - bullet_solver_pub->msg_.yaw_subtract_ = yaw_subtract_; - bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; - bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; - bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.test_number = traject_switch_time_; - bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; - bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; - bullet_solver_pub->msg_.test_number = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - if (v_yaw > 1.0) + for (int i = 0; i < 150; ++i) { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 3 * 2 * M_PI / armors_num); - } - else if (v_yaw < -1.0) - { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 3 * 2 * M_PI / armors_num); + bullet_solver_pub->msg_.selected_armor_[i] = static_cast(selected_armor_[i]); + bullet_solver_pub->msg_.count_number_[i] = static_cast(count_[i]); } + bullet_solver_pub->msg_.start_vel = stauts_limit_.start_vel; + bullet_solver_pub->msg_.switchtime = switchtime; + bullet_solver_pub->msg_.traject_output_yaw_ = traject_output_yaw_; + bullet_solver_pub->msg_.traject_acc_ = traject_effort_ff_; + bullet_solver_pub->msg_.traject_vel_ = filtered_vel_des; + bullet_solver_pub->msg_.using_traject_ = using_traject_; + bullet_solver_pub->msg_.test_number[0] = yaw_[0]; bullet_solver_pub->unlockAndPublish(); } return true; } + void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num) + double v_yaw, double r1, double r2, double dz, double armors_num) { + if (armors_num == 3) + { + armors_num = 3.6; + } double r = r1, z = pos.z; - if (armors_num == 4 && selected_armor_ != 0 && armors_num == 3) + if (armors_num == 4 && selected_armor_[0] % 2 != 0) { r = r2; z = pos.z + dz; } - pos.x += vel.x * (config_.track_move_target_delay + fly_time_); - pos.y += vel.y * (config_.track_move_target_delay + fly_time_); + pos.x += vel.x * (config_.track_move_target_delay + fly_time_[0]); + pos.y += vel.y * (config_.track_move_target_delay + fly_time_[0]); if (track_target_) { - armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); - armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_pos.z = z; armor_vel.x = vel.x + v_yaw * r * - sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.y = vel.y - v_yaw * r * - cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.z = vel.z; } else @@ -470,18 +505,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi double roll{}, pitch{}, yaw{}; quatToRPY(odom2pitch.transform.rotation, roll, pitch, yaw); geometry_msgs::Point point_desire{}, point_real{}; - double target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); + double target_rho = std::sqrt(std::pow(target_pos_[0].x, 2) + std::pow(target_pos_[0].y, 2)); int point_num = int(target_rho * 20); for (int i = 0; i <= point_num; i++) { double rt_bullet_rho = target_rho * i / point_num; - double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / + double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[0])))) / resistance_coff_; - double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * + double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_[0]) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - point_desire.x = rt_bullet_rho * std::cos(output_yaw_) + odom2pitch.transform.translation.x; - point_desire.y = rt_bullet_rho * std::sin(output_yaw_) + odom2pitch.transform.translation.y; + point_desire.x = rt_bullet_rho * std::cos(output_yaw_[0]) + odom2pitch.transform.translation.x; + point_desire.y = rt_bullet_rho * std::sin(output_yaw_[0]) + odom2pitch.transform.translation.y; point_desire.z = rt_bullet_z + odom2pitch.transform.translation.z; marker_desire_.points.push_back(point_desire); } @@ -513,14 +548,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, - double bullet_speed) + double r1, double r2, double dz, double armors_num, double yaw_real, + double pitch_real, double bullet_speed) { + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); double delay; delay = track_target_ ? 0. : config_.center_delay; double r, z; - if (selected_armor_ % 2 == 0) + if (selected_armor_[0] % 2 == 0) { r = r1; z = pos.z; @@ -534,28 +573,28 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec if (track_target_) { double bullet_rho = - bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_; double bullet_x = bullet_rho * std::cos(yaw_real); double bullet_y = bullet_rho * std::sin(yaw_real); double bullet_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; - error = std::sqrt(std::pow(target_pos_.x - bullet_x, 2) + std::pow(target_pos_.y - bullet_y, 2) + - std::pow(target_pos_.z - bullet_z, 2)); + (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_ - + config_.g * fly_time_[0] / resistance_coff_; + error = std::sqrt(std::pow(target_pos_[0].x - bullet_x, 2) + std::pow(target_pos_[0].y - bullet_y, 2) + + std::pow(target_pos_[0].z - bullet_z, 2)); } else { geometry_msgs::Point target_pos_after_fly_time_and_delay{}; target_pos_after_fly_time_and_delay.x = - pos.x + vel.x * (fly_time_ + delay) - - r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + pos.x + vel.x * (fly_time_[0] + delay) - + r * cos(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); target_pos_after_fly_time_and_delay.y = - pos.y + vel.y * (fly_time_ + delay) - - r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); - error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + - std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) + - std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2)); + pos.y + vel.y * (fly_time_[0] + delay) - + r * sin(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_[0] + delay); + error = std::sqrt(std::pow(target_pos_[0].x - target_pos_after_fly_time_and_delay.x, 2) + + std::pow(target_pos_[0].y - target_pos_after_fly_time_and_delay.y, 2) + + std::pow(target_pos_[0].z - target_pos_after_fly_time_and_delay.z, 2)); } return error; } @@ -566,47 +605,53 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) identified_target_change_ = true; else if (!msg->data && identified_target_change_) { - change_armor = true; identified_target_change_ = false; } } -double BulletSolver::getGimbalSwitchDuration(double v_yaw) -{ - gimbal_switch_duration_ = - config_.switch_duration_scale * std::exp(config_.switch_duration_rate * (v_yaw * 60 / (2 * M_PI))) + - config_.switch_duration_offset; - return gimbal_switch_duration_; -} - -void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) +uint8_t BulletSolver::judgeShootBeforehand(double v_yaw, int id) { if (!track_target_) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel) { - if ((ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) && - (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime)) + int ban_shoot_delay_number = static_cast(55 + config_.delay * 1000); + if (id == 6) + { + ban_shoot_delay_number = static_cast(55 + config_.outpost_delay * 1000); + } + ban_shoot_delay_number = ban_shoot_delay_number > 149 ? 149 : ban_shoot_delay_number; + if (selected_armor_[ban_shoot_delay_number] != selected_armor_[0]) + { + ban_shoot_count_++; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + if (ban_shoot_count_ == 1) + { + ban_shoot_start_time_ = ros::Time::now(); + } + } + else if (using_traject_ && ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (ros::Time::now().toSec() > - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime && - ros::Time::now().toSec() < - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime + config_.delay) + } + else if (ros::Time::now().toSec() > ban_shoot_start_time_.toSec() + switchtime && + ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime + config_.delay) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + ban_shoot_count_ = 0; + } else + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + ban_shoot_count_ = 0; + } } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; - if (shoot_beforehand_cmd_pub_->trylock()) - { - shoot_beforehand_cmd_pub_->msg_.stamp = time; - shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_; - shoot_beforehand_cmd_pub_->unlockAndPublish(); - } + return static_cast(shoot_beforehand_cmd_); } -double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_) +double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw) { double a0 = trajectory_function_coefficients.a0; double a1 = trajectory_function_coefficients.a1; @@ -614,8 +659,8 @@ double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_ double a3 = trajectory_function_coefficients.a3; double n_time_ = (time - start_trajectory_time_).toSec(); double plansetpoint = a0 + a1 * n_time_ + a2 * pow(n_time_, 2) + a3 * pow(n_time_, 3); - // traject_effort_ff_ = 1.4 - 1.4 * n_time_/switchtime; - traject_effort_ff_ = 0.0; + traject_effort_ff_ = (2 * a2 + 6 * a3 * n_time_) / (2 * a2) * 1.0 * config_.traject_k_effort; + traject_vel_ = (a1 + 2 * a2 * n_time_ + 3 * a3 * pow(n_time_, 2)) * config_.traject_k_vel_; return plansetpoint; } @@ -646,20 +691,22 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30; config.g = init_config.g; config.delay = init_config.delay; + config.outpost_delay = init_config.outpost_delay; config.center_delay = init_config.center_delay; - config.max_switch_angle = init_config.max_switch_angle; config.switch_angle_offset = init_config.switch_angle_offset; - config.switch_duration_scale = init_config.switch_duration_scale; - config.switch_duration_rate = init_config.switch_duration_rate; - config.switch_duration_offset = init_config.switch_duration_offset; + config.outpost_switch_angle_offset = init_config.outpost_switch_angle_offset; config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; + config.yaw_max_acc = init_config.yaw_max_acc; + config.track_rotate_outpost_delay = init_config.track_rotate_outpost_delay; config.min_fit_switch_count = init_config.min_fit_switch_count; - config.max_selected_armor_ = init_config.max_selected_armor_; + config.traject_start_fit_ = init_config.traject_start_fit_; config.traject_ahead_ = init_config.traject_ahead_; config.clean_shoot_num_ = init_config.clean_shoot_num_; config.end_pos_offset = init_config.end_pos_offset; + config.traject_k_effort = init_config.traject_k_effort; + config.traject_k_vel = init_config.traject_k_vel_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -667,22 +714,25 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_16 = config.resistance_coff_qd_16, .resistance_coff_qd_18 = config.resistance_coff_qd_18, .resistance_coff_qd_30 = config.resistance_coff_qd_30, + .resistance_coff_qd_800 = config.resistance_coff_qd_800, .g = config.g, .delay = config.delay, + .outpost_delay = config.outpost_delay, .center_delay = config.center_delay, - .max_switch_angle = config.max_switch_angle, .switch_angle_offset = config.switch_angle_offset, - .switch_duration_scale = config.switch_duration_scale, - .switch_duration_rate = config.switch_duration_rate, - .switch_duration_offset = config.switch_duration_offset, + .outpost_switch_angle_offset = config.outpost_switch_angle_offset, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, + .yaw_max_acc = config.yaw_max_acc, + .track_rotate_outpost_delay = config.track_rotate_outpost_delay, .min_fit_switch_count = config.min_fit_switch_count, - .max_selected_armor_ = config.max_selected_armor_, + .traject_start_fit_ = config.traject_start_fit_, .traject_ahead_ = config.traject_ahead_, .clean_shoot_num_ = config.clean_shoot_num_, - .end_pos_offset = config.end_pos_offset }; + .end_pos_offset = config.end_pos_offset, + .traject_k_effort = config.traject_k_effort, + .traject_k_vel_ = config.traject_k_vel }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/dual_yaw_controller.cpp b/rm_gimbal_controllers/src/dual_yaw_controller.cpp new file mode 100644 index 00000000..69ffa1b4 --- /dev/null +++ b/rm_gimbal_controllers/src/dual_yaw_controller.cpp @@ -0,0 +1,154 @@ +#include "rm_gimbal_controllers/dual_yaw_controller.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace rm_gimbal_controllers +{ +bool DualYawController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + if (!initBaseYaw(robot_hw, controller_nh)) + return false; + return Controller::init(robot_hw, root_nh, controller_nh); +} + +bool DualYawController::initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh) +{ + ros::NodeHandle base_yaw_nh(controller_nh, "controllers/base_yaw"); + ros::NodeHandle base_yaw_pid_nh(controller_nh, "controllers/base_yaw/pid_pos"); + + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + base_yaw_joint_ = urdf.getJoint(getParam(base_yaw_nh, "joint", std::string("base_yaw_joint"))); + if (!base_yaw_joint_) + { + ROS_ERROR("Could not find base_yaw joint in urdf"); + return false; + } + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); + base_yaw_ctrl_ = std::make_unique(); + base_yaw_pid_pos_ = std::make_unique(); + base_yaw_pos_state_pub_ = + std::make_unique>(base_yaw_nh, "pos_state", 1); + return base_yaw_ctrl_->init(effort_joint_interface, base_yaw_nh) && base_yaw_pid_pos_->init(base_yaw_pid_nh); +} + +bool DualYawController::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& joint_urdf, int axis) const +{ + return !(axis == 2 && base_yaw_joint_ && joint_urdf->name == base_yaw_joint_->name); +} + +bool DualYawController::shouldApplyJointLimit(int axis) const +{ + return axis != 2; +} + +std::string DualYawController::getBaseFrameID(const std::unordered_map& joint_urdfs) +{ + if (base_yaw_joint_) + return base_yaw_joint_->parent_link_name.c_str(); + return Controller::getBaseFrameID(joint_urdfs); +} + +void DualYawController::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double /*pos_des_temp*/[3], + const double vel_des[3], const double angle_error[3], + const double traject_pos_des[3], const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) + { + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); + } + + if (!base_yaw_ctrl_ || !base_yaw_pid_pos_) + return; + + double base_yaw_pos_real = 0.; + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + double roll{}, pitch{}; + quatToRPY(odom2base_yaw.transform.rotation, roll, pitch, base_yaw_pos_real); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + + double armor_set_point{}; + double base_yaw_set_point = getTrackArmorSetPoint(time, armor_set_point) ? armor_set_point : pos_des[2]; + double base_yaw_error = angles::shortest_angular_distance(base_yaw_pos_real, base_yaw_set_point); + base_yaw_pid_pos_->computeCommand(base_yaw_error, period); + base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() - chassis_vel_->angular_->z()); + base_yaw_ctrl_->update(time, period); + publishBaseYawState(time, base_yaw_pos_real, base_yaw_set_point, vel_des, base_yaw_error); +} + +bool DualYawController::getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point) +{ + if (state_ != TRACK || !base_yaw_joint_ || data_track_.header.stamp.isZero()) + return false; + + geometry_msgs::Point target_pos = data_track_.position; + if (!std::isfinite(target_pos.x) || !std::isfinite(target_pos.y) || !std::isfinite(target_pos.z)) + return false; + + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + geometry_msgs::Point base_yaw_pos_odom; + base_yaw_pos_odom.x = odom2base_yaw.transform.translation.x; + base_yaw_pos_odom.y = odom2base_yaw.transform.translation.y; + base_yaw_pos_odom.z = odom2base_yaw.transform.translation.z; + + armor_set_point = std::atan2(target_pos.y - base_yaw_pos_odom.y, target_pos.x - base_yaw_pos_odom.x); + if (!std::isfinite(armor_set_point)) + return false; + + return true; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } +} + +void DualYawController::publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error) +{ + if (base_yaw_pos_state_pub_ && loop_count_ % 10 == 0 && base_yaw_pos_state_pub_->trylock()) + { + base_yaw_pos_state_pub_->msg_.header.stamp = time; + base_yaw_pos_state_pub_->msg_.set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.traject_set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.set_point_dot = vel_des[2]; + base_yaw_pos_state_pub_->msg_.process_value = base_yaw_pos_real; + base_yaw_pos_state_pub_->msg_.error = base_yaw_error; + base_yaw_pos_state_pub_->msg_.command = base_yaw_pid_pos_->getCurrentCmd(); + base_yaw_pos_state_pub_->msg_.shoot_number = bullet_solver_->getShootnum(); + base_yaw_pos_state_pub_->unlockAndPublish(); + } +} + +} // namespace rm_gimbal_controllers + +PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::DualYawController, controller_interface::ControllerBase) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e7c67730..70125f48 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -36,6 +36,7 @@ // #include "rm_gimbal_controllers/gimbal_base.h" +#include #include #include #include @@ -48,27 +49,6 @@ namespace rm_gimbal_controllers { bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); - chassis_vel_ = std::make_shared(chassis_vel_nh); - ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); - bullet_solver_ = std::make_shared(nh_bullet_solver); - ros::NodeHandle nh_ballistic_solver = ros::NodeHandle(controller_nh, "ballistic_solver"); - ballistic_solver_ = std::make_shared(nh_ballistic_solver); - - config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), - .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), - .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), - .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), - .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), - .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), - .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.) }; - config_rt_buffer_.initRT(config_); - d_srv_ = new dynamic_reconfigure::Server(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - XmlRpc::XmlRpcValue xml_rpc_value; const bool enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); if (enable_feedforward) @@ -82,7 +62,60 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - auto* effort_joint_interface = robot_hw->get(); + ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); + chassis_vel_ = std::make_shared(chassis_vel_nh); + ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); + bullet_solver_ = std::make_shared(nh_bullet_solver); + ros::NodeHandle nh_ballistic_solver = ros::NodeHandle(controller_nh, "ballistic_solver"); + ballistic_solver_ = std::make_shared(nh_ballistic_solver); + + std::string track_solver_type; + controller_nh.param("track_solver_type", track_solver_type, "bullet_solver"); + if (track_solver_type == "external_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_WARN("[Gimbal] track_solver_type=external_aim is deprecated, using external_mpc_aim instead"); + } + else if (track_solver_type == "external_mpc_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_INFO("[Gimbal] TRACK mode uses external MPC aim solver"); + } + else if (track_solver_type != "bullet_solver") + { + track_solver_type_ = TrackSolverType::BULLET_SOLVER; + ROS_WARN("[Gimbal] Unknown track_solver_type '%s', falling back to bullet_solver", track_solver_type.c_str()); + } + controller_nh.param("external_aim_timeout", external_aim_timeout_, 0.1); + std::string external_mpc_aim_topic; + controller_nh.param("external_mpc_aim_topic", external_mpc_aim_topic, "/sp_vision/mpc_gimbal_aim"); + + config_ = { + .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), + .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), + .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), + .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), + .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), + .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), + .moment_of_inertia_ = getParam(controller_nh, "moment_of_inertia", 0.), + }; + + config_rt_buffer_.initRT(config_); + d_srv_ = new dynamic_reconfigure::Server(controller_nh); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; + d_srv_->setCallback(cb); + std::string target_is_armor_topic; + controller_nh.param("target_is_armor_topic", target_is_armor_topic, "/vision/target_is_armor"); + controller_nh.param("initial_target_is_armor", target_is_armor_, true); + target_is_armor_pub_ = root_nh.advertise(target_is_armor_topic, 1, true); + publishTargetIsArmor(); + TrackSolver_type_srv_ = root_nh.advertiseService("/Processor/status_change", &Controller::TrackSolver_typeCB, this); + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { // Get URDF info about joint @@ -103,12 +136,15 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro return false; } const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - joint_urdfs_.emplace(axis, std::move(joint_urdf)); - ctrls_.emplace(axis, std::make_unique()); - pid_pos_.emplace(axis, std::make_unique()); - pos_des_in_limit_.emplace(axis, true); - pos_state_pub_.emplace( - axis, std::make_unique>(nh, "pos_state", 1)); + if (!shouldInitializeController(it.first, joint_urdf, axis)) + continue; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + ctrls_.insert(std::make_pair(axis, std::make_unique())); + pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_des_in_limit_.insert(std::make_pair(axis, true)); + pos_state_pub_.insert(std::make_pair( + axis, std::make_unique>(nh, "pos_state", 1))); + if (!ctrls_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) return false; } @@ -119,8 +155,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); - auto* imu_sensor_interface = robot_hw->get(); + hardware_interface::ImuSensorInterface* imu_sensor_interface = + robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); + ROS_WARN_ONCE("has imu"); } else { @@ -141,12 +179,23 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_; odom2gimbal_traject_des_.transform.rotation.w = 1.; + rm_msgs::GimbalCmd default_cmd; + rm_msgs::TrackData default_track; + rm_msgs::MPCGimbalAimCmd default_aim; + cmd_rt_buffer_.initRT(default_cmd); + track_rt_buffer_.initRT(default_track); + gimbal_aim_rt_buffer_.initRT(default_aim); + cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); + data_track_sub_ = controller_nh.subscribe("/sp_vision/track", 1, &Controller::trackCB, this); ballistic_solver_request_sub_ = controller_nh.subscribe("/ballistic_solver_request", 1, &Controller::ballisticSolverRequestCB, this); + mpc_gimbal_aim_sub_ = + controller_nh.subscribe(external_mpc_aim_topic, 1, &Controller::mpcGimbalAimCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); ballistic_solution_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "ballistic_solution", 100)); @@ -164,6 +213,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); try { @@ -189,7 +239,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) rate(time, period); break; case TRACK: - track(time); + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) + externalAimTrack(time); + else + track(time, period); break; case DIRECT: direct(time); @@ -201,24 +254,40 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } +bool Controller::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const +{ + return true; +} + +bool Controller::shouldApplyJointLimit(int /*axis*/) const +{ + return true; +} + void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { - tf2::Quaternion odom2base, odom2gimbal_des, base2gimbal_des, base2gimbal_traject_des; + tf2::Quaternion odom2base, odom2gimbal_des_q, base2gimbal_des, base2gimbal_traject_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); - tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des); + tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des_q); double current_rpy[3], des_rpy[3], traject_rpy[3]; - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), current_rpy[0], current_rpy[1], current_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), des_rpy[0], des_rpy[1], des_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, traject_yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), current_rpy[0], current_rpy[1], current_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), des_rpy[0], des_rpy[1], des_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, traject_yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + for (const auto& it : joint_urdfs_) { - bool update = (it.first != 1 && it.first != 2) || (it.first == 1 && update_pitch) || (it.first == 2 && update_yaw); - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); + if (shouldApplyJointLimit(it.first)) + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + else + pos_des_in_limit_[it.first] = true; } + base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); base2gimbal_traject_des.setRPY(traject_rpy[0], traject_rpy[1], traject_rpy[2]); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); @@ -229,6 +298,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, void Controller::rate(const ros::Time& time, const ros::Duration& period) { + bullet_solver_->CleanTrackCount(); if (state_changed_) { // on enter state_changed_ = false; @@ -254,8 +324,16 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) } } -void Controller::track(const ros::Time& time) +void Controller::track(const ros::Time& time, const ros::Duration& period) { + external_aim_active_ = false; + if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) + { + state_ = RATE; + state_changed_ = true; + rate(time, period); + return; + } if (state_changed_) { // on enter state_changed_ = false; @@ -273,7 +351,7 @@ void Controller::track(const ros::Time& time) { if (!data_track_.header.frame_id.empty()) { - auto transform = + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("odom", data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); @@ -296,8 +374,8 @@ void Controller::track(const ros::Time& time) target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity()); - bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); + data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); + publishShootBeforehand(time, bullet_solver_->judgeShootBeforehand(data_track_.v_yaw, data_track_.id)); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { @@ -324,6 +402,58 @@ void Controller::track(const ros::Time& time) } } +bool Controller::externalAimIsFresh(const ros::Time& time) const +{ + if (!gimbal_aim_.valid || gimbal_aim_.header.stamp.isZero()) + return false; + const double age = (time - gimbal_aim_.header.stamp).toSec(); + return age >= -external_aim_timeout_ && age <= external_aim_timeout_; +} + +void Controller::externalAimTrack(const ros::Time& time) +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Gimbal] Enter TRACK with external aim"); + } + + external_aim_active_ = externalAimIsFresh(time); + if (external_aim_active_) + { + setDes(time, gimbal_aim_.target_yaw, gimbal_aim_.pitch, gimbal_aim_.yaw); + publishShootBeforehand(time, gimbal_aim_.shoot_cmd == 1 ? rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT : + rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + else + { + odom2gimbal_des_.header.stamp = time; + robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); + publishShootBeforehand(time, rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + { + if (error_pub_->trylock()) + { + error_pub_->msg_.stamp = time; + error_pub_->msg_.error = external_aim_active_ && std::isfinite(gimbal_aim_.error) ? gimbal_aim_.error : 1.0; + error_pub_->unlockAndPublish(); + } + last_publish_time_ = time; + } +} + +void Controller::publishShootBeforehand(const ros::Time& time, uint8_t cmd) +{ + if (shoot_beforehand_cmd_pub_ && shoot_beforehand_cmd_pub_->trylock()) + { + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = cmd; + shoot_beforehand_cmd_pub_->unlockAndPublish(); + } +} + void Controller::direct(const ros::Time& time) { if (state_changed_) @@ -335,11 +465,9 @@ void Controller::direct(const ros::Time& time) try { if (!cmd_gimbal_.target_pos.header.frame_id.empty()) - { - auto transform = robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, - cmd_gimbal_.target_pos.header.stamp); - tf2::doTransform(aim_point_odom, aim_point_odom, transform); - } + tf2::doTransform(aim_point_odom, aim_point_odom, + robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, + cmd_gimbal_.target_pos.header.stamp)); } catch (tf2::TransformException& ex) { @@ -419,9 +547,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) gyro.z = imu_sensor_handle_.getAngularVelocity()[2]; try { - auto transform = - robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), time); - tf2::doTransform(gyro, angular_vel, transform); + tf2::doTransform(gyro, angular_vel, + robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), + time)); } catch (tf2::TransformException& ex) { @@ -438,6 +566,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrls_.find(2) != ctrls_.end()) angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } + gimbal_real_z_vel_ = angular_vel.z; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); @@ -453,79 +582,83 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - if (data_track_.id != 12) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) { - geometry_msgs::Point pos = data_track_.position; - double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); - pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); - pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); - pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, - data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, - data_track_.dz, data_track_.armors_num); + if (external_aim_active_ && externalAimIsFresh(time)) + { + vel_des[2] = gimbal_aim_.yaw_rate; + vel_des[1] = gimbal_aim_.pitch_rate; + } + else + { + vel_des[2] = 0.; + vel_des[1] = 0.; + } } else { - target_pos = data_track_.position; - target_vel = data_track_.velocity; - } - target_vel.x -= chassis_vel_->linear_->x(); - target_vel.y -= chassis_vel_->linear_->y(); - target_vel.z -= chassis_vel_->linear_->z(); - tf2::Vector3 target_pos_tf, target_vel_tf; - try - { - if (joint_urdfs_.find(2) != joint_urdfs_.end()) + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + if (data_track_.id != 12) { - auto transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + geometry_msgs::Point pos = data_track_.position; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); + pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); + pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, + data_track_.dz, data_track_.armors_num); } - if (joint_urdfs_.find(1) != joint_urdfs_.end()) + else { - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + target_pos = data_track_.position; + target_vel = data_track_.velocity; + } + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); + tf2::Vector3 target_pos_tf, target_vel_tf; + try + { + if (joint_urdfs_.find(2) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + } + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + { + vel_des[2] = bullet_solver_->getTrajectVel(); + } + if (joint_urdfs_.find(1) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); } - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); } } + for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); - } - if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) - { - pid_pos_.at(2)->computeCommand(bullet_solver_->getUsingtraject() ? traject_angle_error[2] : angle_error[2], period); - double cmd = pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z; - if (state_ == TRACK && bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) - cmd += bullet_solver_->getTrajectEffortff(); - ctrls_.at(2)->setCommand(cmd); - ctrls_.at(2)->update(time, period); - } + updatePitchJoint(time, period, angular_vel, vel_des, angle_error); + updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, + traject_angle_error); // publish state if (loop_count_ % 10 == 0) @@ -561,9 +694,9 @@ double Controller::gravityFeedForward(const ros::Time& time) if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, - joint_urdfs_.at(1)->parent_link_name, time); - tf2::doTransform(gravity_compensation, gravity_compensation, transform); + tf2::doTransform(gravity_compensation, gravity_compensation, + robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, + joint_urdfs_.at(1)->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -606,27 +739,97 @@ void Controller::updateBallisticSolution(const ros::Time& time) last_ballistic_publish_time_ = time; } } +void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]) +{ + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) + { + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); + } +} + +void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], + const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], + const double angle_error[3], const double /*traject_pos_des*/[3], + const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) + return; -std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); +} + +void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, + control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], + const double angle_error[3], const double traject_angle_error[3]) +{ + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) + { + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); + ctrls_.at(2)->joint_.setCommand(cmd); + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); + } + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); + } +} + +std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->child_link_name; + return joint_urdfs.at(1)->child_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->child_link_name; + return joint_urdfs.at(0)->child_link_name.c_str(); if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->child_link_name; - return {}; + return joint_urdfs.at(2)->child_link_name.c_str(); + return std::string(); } -std::string Controller::getBaseFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->parent_link_name; + return joint_urdfs.at(2)->parent_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->parent_link_name; + return joint_urdfs.at(0)->parent_link_name.c_str(); if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->parent_link_name; - return {}; + return joint_urdfs.at(1)->parent_link_name.c_str(); + return std::string(); } double Controller::updateCompensation(double chassis_vel_angular_z) @@ -656,6 +859,28 @@ void Controller::ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg) if (ballistic_solver_request) ballistic_solver_->solver(base2gimbal_, ballistic_yaw_, ballistic_pitch_); } +void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) +{ + gimbal_aim_rt_buffer_.writeFromNonRT(*msg); +} + +bool Controller::TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res) +{ + const bool previous = target_is_armor_; + target_is_armor_ = req.target == rm_msgs::StatusChangeRequest::ARMOR; + publishTargetIsArmor(); + if (previous != target_is_armor_) + ROS_INFO("[Gimbal] armor vision pipeline %s", target_is_armor_ ? "enabled" : "disabled"); + res.switch_is_success = true; + return true; +} + +void Controller::publishTargetIsArmor() +{ + std_msgs::Bool msg; + msg.data = target_is_armor_; + target_is_armor_pub_.publish(msg); +} void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) { @@ -671,6 +896,9 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin config.chassis_comp_b_ = init_config.chassis_comp_b_; config.chassis_comp_c_ = init_config.chassis_comp_c_; config.chassis_comp_d_ = init_config.chassis_comp_d_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; + config.moment_of_inertia_ = init_config.moment_of_inertia_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, @@ -680,7 +908,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, - .chassis_comp_d_ = config.chassis_comp_d_ }; + .chassis_comp_d_ = config.chassis_comp_d_, + .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } diff --git a/rm_gimbal_controllers/test/gimbal_config_template.yaml b/rm_gimbal_controllers/test/gimbal_config_template.yaml deleted file mode 100644 index 37e9b3ab..00000000 --- a/rm_gimbal_controllers/test/gimbal_config_template.yaml +++ /dev/null @@ -1,32 +0,0 @@ -controllers: - robot_state_controller: - type: robot_state_controller/RobotStateController - publish_rate: 50 - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - gimbal_controller: - type: rm_gimbal_controllers/Controller - yaw: - joint: "yaw_joint" - pid: { p: 7.5, i: 0, d: 0.3, i_clamp_max: 0.3, i_clamp_min: -0.3, antiwindup: true, publish_state: true } - k_chassis_vel: 0.01 - pitch: - joint: "pitch_joint" - pid: { p: 22.0, i: 0, d: 0.75, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - imu_name: "gimbal_imu" - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 5.0 - g: 9.81 - delay: 0.15 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 diff --git a/rm_gimbal_controllers/test/load_controllers.launch b/rm_gimbal_controllers/test/load_controllers.launch deleted file mode 100644 index e2eb54f5..00000000 --- a/rm_gimbal_controllers/test/load_controllers.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - diff --git a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h index b03ac226..ef653261 100644 --- a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h +++ b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h @@ -10,6 +10,8 @@ #include #include #include +#include +#include namespace rm_orientation_controller { @@ -25,6 +27,7 @@ class Controller : public controller_interface::MultiInterfaceController> assembly_error_pub_; bool receive_imu_msg_ = false; + int loop_count_{}; }; } // namespace rm_orientation_controller diff --git a/rm_orientation_controller/src/orientation_controller.cpp b/rm_orientation_controller/src/orientation_controller.cpp index 35640f02..1db309ed 100644 --- a/rm_orientation_controller/src/orientation_controller.cpp +++ b/rm_orientation_controller/src/orientation_controller.cpp @@ -23,6 +23,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro tf_broadcaster_.init(root_nh); imu_data_sub_ = root_nh.subscribe("data", 1, &Controller::imuDataCallback, this); + assembly_error_pub_.reset( + new realtime_tools::RealtimePublisher(root_nh, "imu_assembly_error", 10)); source2target_msg_.header.frame_id = frame_source_; source2target_msg_.child_frame_id = frame_target_; source2target_msg_.transform.rotation.w = 1.0; @@ -48,6 +50,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) if (!receive_imu_msg_) tf_broadcaster_.sendTransform(source2target_msg_); } + AssemblyErrorPub(time); } bool Controller::getTransform(const ros::Time& time, geometry_msgs::TransformStamped& source2target, const double x, @@ -90,6 +93,53 @@ void Controller::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) tf_broadcaster_.sendTransform(source2target); } +void Controller::AssemblyErrorPub(const ros::Time& time) +{ + double roll_error, pitch_error, yaw_error, roll_imu, pitch_imu, yaw_imu; + geometry_msgs::TransformStamped tf_source2target, tf_target2imu; + tf2::Transform source2target, target2imu; + Eigen::Matrix3d roll_eigen, pitch_eigen, yaw_eigen, R_eigen; + Eigen::Vector3d error_eigen, assembly_error_eigen; + try + { + tf_source2target = robot_state_.lookupTransform(frame_source_, frame_target_, time); + tf_target2imu = robot_state_.lookupTransform(frame_target_, imu_sensor_.getFrameId(), time); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + } + tf2::fromMsg(tf_source2target.transform, source2target); + tf2::Matrix3x3(source2target.getRotation()).getRPY(roll_error, pitch_error, yaw_error); + tf2::fromMsg(tf_target2imu.transform, target2imu); + tf2::Matrix3x3(target2imu.getRotation()).getRPY(roll_imu, pitch_imu, yaw_imu); + roll_imu = static_cast(roll_imu * 2 / M_PI); + pitch_imu = static_cast(pitch_imu * 2 / M_PI); + yaw_imu = static_cast(yaw_imu * 2 / M_PI); + roll_eigen << 1, 0, 0, 0, cos(roll_imu * M_PI / 2), -sin(roll_imu * M_PI / 2), 0, sin(roll_imu * M_PI / 2), + cos(roll_imu * M_PI / 2); + pitch_eigen << cos(pitch_imu * M_PI / 2), 0, sin(pitch_imu * M_PI / 2), 0, 1, 0, -sin(pitch_imu * M_PI / 2), 0, + cos(pitch_imu * M_PI / 2); + yaw_eigen << cos(yaw_imu * M_PI / 2), -sin(yaw_imu * M_PI / 2), 0, sin(yaw_imu * M_PI / 2), cos(yaw_imu * M_PI / 2), + 0, 0, 0, 1; + R_eigen = roll_eigen * pitch_eigen * yaw_eigen; + error_eigen << roll_error, pitch_error, 0.0; + assembly_error_eigen = R_eigen * error_eigen; + if (loop_count_ % 100 == 0) + { + if (assembly_error_pub_->trylock()) + { + assembly_error_pub_->msg_.header.stamp = time; + assembly_error_pub_->msg_.roll_error = assembly_error_eigen[0]; + assembly_error_pub_->msg_.pitch_error = assembly_error_eigen[1]; + assembly_error_pub_->msg_.yaw_error = assembly_error_eigen[2]; + assembly_error_pub_->unlockAndPublish(); + } + else + ROS_WARN_THROTTLE(1, "Can't publish assembly error data"); + } + loop_count_++; +} } // namespace rm_orientation_controller PLUGINLIB_EXPORT_CLASS(rm_orientation_controller::Controller, controller_interface::ControllerBase) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index a68f830c..24bcf6cf 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -345,7 +345,8 @@ void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& pe double friction_change_speed_derivative = friction_change_speed - last_friction_change_speed_; if (state_ != STOP) { - if (friction_change_speed_derivative > 0 && has_shoot_) + if (friction_change_speed_derivative > 0 && has_shoot_ && + friction_change_speed > config_.wheel_speed_raise_threshold) has_shoot_ = false; if (friction_change_speed < -config_.wheel_speed_drop_threshold && !has_shoot_ && friction_change_speed_derivative < 0)