diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml old mode 100644 new mode 100755 diff --git a/include/rm_manual/dart2_manual.h b/include/rm_manual/dart2_manual.h index 069734d8..bf9ca1a9 100644 --- a/include/rm_manual/dart2_manual.h +++ b/include/rm_manual/dart2_manual.h @@ -34,22 +34,10 @@ class Dart2Manual : public ManualBase { INIT = 0, PULLDOWN = 1, - ROTATE_PLACE = 2, - LOADING = 3, - ROTATE_BACK = 4, - LOADED = 5, - ENGAGE = 6, - PULLUP = 7, - READY = 8, - PUSH = 9 - }; - enum ClampType - { - ZERO = 0, - LEFT = 1, - MID = 2, - RIGHT = 3, - ALL = 4 + ENGAGE = 2, + PULLUP = 3, + READY = 4, + PUSH = 5 }; enum AutoAimState { @@ -60,11 +48,7 @@ class Dart2Manual : public ManualBase ADJUSTED, FINISH }; - enum ClampMode - { - ClAMP, - RELEASE - }; + struct Dart { double outpost_offset_, base_offset_; @@ -74,7 +58,7 @@ class Dart2Manual : public ManualBase protected: void sendCommand(const ros::Time& time) override; void getList(const XmlRpc::XmlRpcValue& darts, const XmlRpc::XmlRpcValue& targets, - const XmlRpc::XmlRpcValue& launch_id, const XmlRpc::XmlRpcValue& rotate_positions); + const XmlRpc::XmlRpcValue& launch_id); void run() override; void checkReferee() override; void remoteControlTurnOn() override; @@ -86,7 +70,6 @@ class Dart2Manual : public ManualBase void rightSwitchUpRise() override; void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override; - void updateGripper(); void move(rm_common::JointPointCommandSender* joint, double ch); void recordPosition(const rm_msgs::DbusData dbus_data); void readyLaunchDart(int dart_fired_num); @@ -100,93 +83,113 @@ class Dart2Manual : public ManualBase void updateAllowDartDoorOpenTimes(); void wheelClockwise(); void wheelAntiClockwise(); - void setGripperAction(int dart_fired_num); - void changeGripperState(rm_common::JointPointCommandSender* gripper, bool& is_release); - void changeAllGripperState(); - void operateGripper(const rm_msgs::DbusData::ConstPtr& dbus_data); - void longCameraDataCallback(const rm_msgs::Dart::ConstPtr& data); - void shortCameraDataCallback(const rm_msgs::Dart::ConstPtr& data); + + void load_dart(int step_idx); + void load_work(); + void operateClamper(const rm_msgs::DbusData::ConstPtr& dbus_data); + void updateLaunchMode(); - void updateAutoAimState(); void init(); void pullDown(); - void rotatePlace(); - void loading(); - void rotateBack(); - void loaded(); void pullUp(); void engage(); void ready(); void push(); + + // void longCameraDataCallback(const rm_msgs::Dart::ConstPtr& data); + void shortCameraDataCallback(const rm_msgs::Dart::ConstPtr& data); + void updateAutoAimState(); void aim(); void adjust(); void autoAim(); rm_common::JointPointCommandSender* yaw_sender_; rm_common::JointPointCommandSender* trigger_sender_; - rm_common::JointPointCommandSender* rotate_sender_; - rm_common::JointPointCommandSender *clamp_left_sender_, *clamp_right_sender_, *clamp_mid_sender_; rm_common::JointPointCommandSender *belt_left_sender_, *belt_right_sender_; rm_common::JointPointCommandSender* range_sender_; + rm_common::JointPointCommandSender* load_sender_; + rm_common::JointPointCommandSender *clamp_left_sender_, *clamp_mid_sender_, *clamp_right_sender_; rm_common::CalibrationQueue *shooter_calibration_, *gimbal_calibration_, *clamp_calibration_; double range_outpost_{}, range_base_{}, yaw_outpost_{}, yaw_base_{}; - double downward_vel_, upward_vel_; + double belt_left_downward_vel_, belt_right_downward_vel_, belt_left_upward_vel_, belt_right_upward_vel_, + belt_left_upward_slow_vel_, belt_right_upward_slow_vel_; std::unordered_map dart_list_{}; std::unordered_map> target_position_{}; - // std::unordered_map> rotate_position_{}; - std::unordered_map clamp_position_{}; - std::vector rotate_place_position_{}, rotate_back_position_{}; - double scale_{ 0.04 }, scale_micro_{ 0.0005 }; - bool if_stop_{ true }, has_stopped{ false }, is_reach_{ false }, is_calibrate_{ false }, trigger_has_work_{ false }, - ready_{ false }; + double scale_{ 0.05 }, scale_micro_{ 0.0005 }; + bool if_stop_{ true }, has_stopped{ false }, is_reach_{ false }, is_calibrate_{ false }, trigger_has_work_{ false }; int has_fired_num_{}; - bool clamp_calibrate_{ false }; - bool left_clamp_is_release_{ false }, mid_clamp_is_release_{ false }, right_clamp_is_release_{ false }; bool confirm_place_{ false }, confirm_back_{ false }; ros::Time last_time_{}; uint8_t launch_mode_{ 0 }, last_launch_mode_{ 6 }; uint8_t auto_aim_state_{ 0 }, last_auto_aim_state_{}; - uint8_t gripper_state_{ 0 }, last_gripper_state_{ 0 }; rm_msgs::DbusData dbus_data_; uint8_t robot_id_, game_progress_{}, dart_launch_opening_status_{ 3 }; uint16_t remain_time_{ 420 }; int dart_fired_num_ = 0; + double current_x_offset_ = 0.0; + double target_x_offset = 0.0; + double current_y_offset_ = 0.0; + double target_y_offset_ = 0.0; double trigger_home_command_{}, trigger_work_command_{}; double trigger_confirm_home_{}, trigger_confirm_work_{}; double belt_left_position_{}, belt_right_position_{}, trigger_position_{}; - double belt_left_max_{}, belt_right_max_{}, belt_left_min_{}, belt_right_min_{}, belt_left_place_{}, - belt_right_place_{}, belt_left_placed_{}, belt_right_placed_{}; - double clamp_left_position_, clamp_mid_position_, clamp_right_position_, release_position_, clamp_finish_position_; - double range_velocity_ = 0., yaw_velocity_ = 0., rotate_velocity_ = 0.; + double belt_left_max_{}, belt_right_max_{}, belt_left_min_{}, belt_right_min_{}, belt_left_slow_{}, + belt_right_slow_{}; + double range_velocity_ = 0., yaw_velocity_ = 0., load_velocity_ = 0.; double short_camera_x_set_point_, long_camera_x_set_point_{}, long_camera_y_set_point_{}; + + double camera_x_set_point_, camera_y_set_point_; + double camera_x_, camera_y_; + double camera_fast_p_x_, camera_normal_p_x_, camera_slow_p_x_, camera_retarget_slow_p_x_; + double camera_fast_x_threshold_, camera_normal_x_threshold_, camera_slow_x_threshold_; + double camera_x_before_push_{}, camera_x_after_push_{}; + + double short_camera_x_threshold_, long_camera_x_threshold_, retarget_threshold_; double camera_x_offset_, camera_y_offset_; double long_camera_p_x_, short_camera_p_x_, long_camera_p_y_; + bool is_camera_found_{ false }; + bool aim_failed_{ false }; + bool camera_x_init_{ false }; bool is_long_camera_found_{ false }, is_short_camera_found_{ false }, is_long_camera_aim_{}; double long_camera_x_{}, long_camera_y_{}, short_camera_x_{}, short_camera_y_{}, last_camera_x{}; - double long_camera_x_threshold_, retarget_threshold_; bool camera_central_{}, is_adjust_{}; bool camera_is_online_{}; - bool use_auto_aim_{}, start_aim_{}; - bool allow_push_{ false }; + bool use_auto_aim_{}, start_aim_{}, auto_aim_start_{}; + bool random_fixed_target_{}; bool had_adjust_{}; - bool need_back_{}, check_place_{}; + + bool launch_result_{ false }; + + bool retarget{ false }; + bool use_load_{ false }, load_start_{ false }, load_finished_{ false }, clamp_finished_{ false }; + bool belt_left_ready_{ false }, belt_right_ready_{ false }, dart_ready_{ false }, vision_ready_{ false }; + double clamp_left_release_position_, clamp_mid_release_position_, clamp_right_release_position_, + clamp_default_position_; + double load_init_position_, load_left_position_, load_mid_position_, load_right_position_; + + double current_load_target_{ 0.0 }, cur_pos{ 0.0 }; + double long_camera_x_before_push_{}, long_camera_x_after_push_{}; InputEvent wheel_clockwise_event_, wheel_anticlockwise_event_; ros::Time last_engage_time_{}; - ros::Time last_rotate_time_{}; ros::Time last_loading_time_{}; + ros::Time last_loaded_time_{}; ros::Time last_push_time_{}; ros::Time last_ready_time_{}; ros::Time last_init_time_{}; ros::Time last_get_camera_data_time_{}; ros::Time start_aim_time_{}; ros::Time last_adjust_time_{}; + ros::Time last_clamp_finished_time_{}; + ros::Time load_start_time_{}; + ros::Time load_finish_time_{}; + ros::Time vision_end_time_{}; ros::Subscriber dart_client_cmd_sub_; ros::Subscriber long_camera_data_sub_; ros::Subscriber short_camera_data_sub_; @@ -194,7 +197,16 @@ class Dart2Manual : public ManualBase int outpost_hp_; int allow_dart_door_open_times_ = 0, last_dart_door_status_ = 1; bool triggered_30s_{ false }, triggered_4min_{ false }; - int auto_state_ = BASE, manual_state_ = BASE, move_state_ = NORMAL, launch_state_ = INIT; + int auto_state_ = BASE, manual_state_ = BASE, move_state_ = NORMAL; + int clamp_num_{ 1 }; + + bool temp_clamp_{ true }, temp_load_{ false }; + double temp_load_position_{ 0.01 }; + ros::Time push_failed_time = ros::Time::now(); + bool push_failed = true, push_succeeded = false; + double false_engage_time_{ 1.0 }; + + std::vector trigger_status_; }; } // namespace rm_manual diff --git a/include/rm_manual/dart_manual.h b/include/rm_manual/dart_manual.h old mode 100644 new mode 100755 diff --git a/src/dart2_manual.cpp b/src/dart2_manual.cpp index 3cd8643c..772cd43b 100644 --- a/src/dart2_manual.cpp +++ b/src/dart2_manual.cpp @@ -1,5 +1,5 @@ // -// Created by chen_gr on 2025/3/9. +// Created by Aoalas on 2525/10/5. // #include "rm_manual/dart2_manual.h" @@ -8,12 +8,12 @@ namespace rm_manual { Dart2Manual::Dart2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : ManualBase(nh, nh_referee) { - XmlRpc::XmlRpcValue dart_list, targets, launch_id, rotate_positions; + XmlRpc::XmlRpcValue dart_list, targets, launch_id, steps_xml, trigger_status_rpc; nh.getParam("launch_id", launch_id); nh.getParam("dart_list", dart_list); nh.getParam("targets", targets); - nh.getParam("rotate_positions", rotate_positions); - getList(dart_list, targets, launch_id, rotate_positions); + getList(dart_list, targets, launch_id); + yaw_outpost_ = target_position_["outpost"][0]; range_outpost_ = target_position_["outpost"][1]; yaw_base_ = target_position_["base"][0]; @@ -23,21 +23,39 @@ Dart2Manual::Dart2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : Man yaw_sender_ = new rm_common::JointPointCommandSender(nh_yaw, joint_state_); ros::NodeHandle nh_trigger = ros::NodeHandle(nh, "trigger"); trigger_sender_ = new rm_common::JointPointCommandSender(nh_trigger, joint_state_); - ros::NodeHandle nh_rotate = ros::NodeHandle(nh, "rotate"); - rotate_sender_ = new rm_common::JointPointCommandSender(nh_rotate, joint_state_); + ros::NodeHandle nh_load = ros::NodeHandle(nh, "load"); + load_sender_ = new rm_common::JointPointCommandSender(nh_load, joint_state_); + + nh_load.getParam("use_load", use_load_); + nh_load.getParam("load_init_position", load_init_position_); + nh_load.getParam("load_left_position", load_left_position_); + nh_load.getParam("load_mid_position", load_mid_position_); + nh_load.getParam("load_right_position", load_right_position_); nh_trigger.getParam("trigger_home_command", trigger_home_command_); nh_trigger.getParam("trigger_work_command", trigger_work_command_); nh_trigger.getParam("trigger_confirm_home", trigger_confirm_home_); nh_trigger.getParam("trigger_confirm_work", trigger_confirm_work_); + nh_trigger.getParam("false_engage_time", false_engage_time_); + + if (nh_trigger.getParam("trigger_status", trigger_status_rpc)) + { + ROS_ASSERT(trigger_status_rpc.getType() == XmlRpc::XmlRpcValue::TypeArray); + for (int i = 0; i < trigger_status_rpc.size(); ++i) + { + trigger_status_.push_back(static_cast(trigger_status_rpc[i])); + } + } + else + { + trigger_status_ = { true, true, true, true }; + } ros::NodeHandle nh_clamp = ros::NodeHandle(nh, "clamp_positions"); - nh_clamp.getParam("clamp_left_position", clamp_left_position_); - nh_clamp.getParam("clamp_mid_position", clamp_mid_position_); - nh_clamp.getParam("clamp_right_position", clamp_right_position_); - nh_clamp.getParam("release_position", release_position_); - nh_clamp.getParam("clamp_finish_position", clamp_finish_position_); - nh_clamp.getParam("check_place", check_place_); + nh_clamp.getParam("clamp_left_release_position", clamp_left_release_position_); + nh_clamp.getParam("clamp_mid_release_position", clamp_mid_release_position_); + nh_clamp.getParam("clamp_right_release_position", clamp_right_release_position_); + nh_clamp.getParam("clamp_default_position", clamp_default_position_); ros::NodeHandle nh_belt_left = ros::NodeHandle(nh, "belt_left"); ros::NodeHandle nh_belt_right = ros::NodeHandle(nh, "belt_right"); @@ -45,14 +63,16 @@ Dart2Manual::Dart2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : Man belt_right_sender_ = new rm_common::JointPointCommandSender(nh_belt_right, joint_state_); nh_belt_left.getParam("belt_left_max", belt_left_max_); nh_belt_right.getParam("belt_right_max", belt_right_max_); + nh_belt_left.getParam("belt_left_slow", belt_left_slow_); + nh_belt_right.getParam("belt_right_slow", belt_right_slow_); nh_belt_left.getParam("belt_left_min", belt_left_min_); nh_belt_right.getParam("belt_right_min", belt_right_min_); - nh_belt_left.getParam("belt_left_place", belt_left_place_); - nh_belt_right.getParam("belt_right_place", belt_right_place_); - nh_belt_left.getParam("belt_left_placed", belt_left_placed_); - nh_belt_right.getParam("belt_right_placed", belt_right_placed_); - nh_belt_left.getParam("upward_vel", upward_vel_); - nh_belt_left.getParam("downward_vel", downward_vel_); + nh_belt_left.getParam("belt_left_downward_vel", belt_left_downward_vel_); + nh_belt_right.getParam("belt_right_downward_vel", belt_right_downward_vel_); + nh_belt_left.getParam("belt_left_upward_vel", belt_left_upward_vel_); + nh_belt_right.getParam("belt_right_upward_vel", belt_right_upward_vel_); + nh_belt_left.getParam("belt_left_upward_slow_vel", belt_left_upward_slow_vel_); + nh_belt_right.getParam("belt_right_upward_slow_vel", belt_right_upward_slow_vel_); ros::NodeHandle nh_range = ros::NodeHandle(nh, "range"); range_sender_ = new rm_common::JointPointCommandSender(nh_range, joint_state_); @@ -63,9 +83,6 @@ Dart2Manual::Dart2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : Man clamp_left_sender_ = new rm_common::JointPointCommandSender(nh_clamp_left, joint_state_); clamp_mid_sender_ = new rm_common::JointPointCommandSender(nh_clamp_mid, joint_state_); clamp_right_sender_ = new rm_common::JointPointCommandSender(nh_clamp_right, joint_state_); - clamp_position_.insert(std::make_pair(clamp_left_sender_, clamp_left_position_)); - clamp_position_.insert(std::make_pair(clamp_mid_sender_, clamp_mid_position_)); - clamp_position_.insert(std::make_pair(clamp_right_sender_, clamp_right_position_)); XmlRpc::XmlRpcValue shooter_rpc_value, gimbal_rpc_value, clamp_rpc_value; nh.getParam("shooter_calibration", shooter_rpc_value); @@ -78,13 +95,23 @@ Dart2Manual::Dart2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : Man ros::NodeHandle nh_camera = ros::NodeHandle(nh, "camera"); nh_camera.getParam("camera_x_offset", camera_x_offset_); nh_camera.getParam("camera_y_offset", camera_y_offset_); - nh_camera.getParam("long_camera_p_x", long_camera_p_x_); - nh_camera.getParam("long_camera_p_y", long_camera_p_y_); - nh_camera.getParam("short_camera_p_x", short_camera_p_x_); - nh_camera.getParam("long_camera_x_threshold", long_camera_x_threshold_); + // nh_camera.getParam("long_camera_p_x", long_camera_p_x_); + // nh_camera.getParam("short_camera_p_x", short_camera_p_x_); + + nh_camera.getParam("camera_fast_p_x", camera_fast_p_x_); + nh_camera.getParam("camera_normal_p_x", camera_normal_p_x_); + nh_camera.getParam("camera_slow_p_x", camera_slow_p_x_); + nh_camera.getParam("camera_retarget_slow_p_x", camera_retarget_slow_p_x_); + nh_camera.getParam("camera_fast_x_threshold", camera_fast_x_threshold_); + nh_camera.getParam("camera_normal_x_threshold", camera_normal_x_threshold_); + nh_camera.getParam("camera_slow_x_threshold", camera_slow_x_threshold_); nh_camera.getParam("retarget_threshold", retarget_threshold_); + + nh_camera.getParam("long_camera_x_threshold", long_camera_x_threshold_); nh_camera.getParam("use_auto_aim", use_auto_aim_); + nh_camera.getParam("random_fixed_target", random_fixed_target_); + left_switch_up_event_.setActiveHigh(boost::bind(&Dart2Manual::leftSwitchUpOn, this)); left_switch_mid_event_.setActiveHigh(boost::bind(&Dart2Manual::leftSwitchMidOn, this)); left_switch_down_event_.setActiveHigh(boost::bind(&Dart2Manual::leftSwitchDownOn, this)); @@ -100,14 +127,13 @@ Dart2Manual::Dart2Manual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee) : Man nh_referee.subscribe("game_robot_hp", 10, &Dart2Manual::gameRobotHpCallback, this); game_status_sub_ = nh_referee.subscribe("game_status", 10, &Dart2Manual::gameStatusCallback, this); + short_camera_data_sub_ = nh.subscribe("/short_dart_camera_distance", 10, &Dart2Manual::shortCameraDataCallback, this); - long_camera_data_sub_ = - nh.subscribe("/dart_camera_distance", 10, &Dart2Manual::longCameraDataCallback, this); } void Dart2Manual::getList(const XmlRpc::XmlRpcValue& darts, const XmlRpc::XmlRpcValue& targets, - const XmlRpc::XmlRpcValue& launch_id, const XmlRpc::XmlRpcValue& rotate_positions) + const XmlRpc::XmlRpcValue& launch_id) { for (const auto& dart : darts) { @@ -127,6 +153,7 @@ void Dart2Manual::getList(const XmlRpc::XmlRpcValue& darts, const XmlRpc::XmlRpc } } } + for (const auto& target : targets) { ROS_ASSERT(target.second.hasMember("position")); @@ -136,35 +163,12 @@ void Dart2Manual::getList(const XmlRpc::XmlRpcValue& darts, const XmlRpc::XmlRpc position[1] = static_cast(target.second["position"][1]); target_position_.insert(std::make_pair(target.first, position)); } - ROS_ASSERT(rotate_positions.getType() == XmlRpc::XmlRpcValue::TypeArray); - for (int i = 0; i < rotate_positions.size(); i++) - { - auto& position = rotate_positions[i]; - ROS_ASSERT(position.hasMember("angle")); - auto& angle_val = position["angle"]; - ROS_ASSERT(angle_val.getType() == XmlRpc::XmlRpcValue::TypeArray); - ROS_ASSERT(angle_val.size() >= 2); - auto angle0 = static_cast(angle_val[0]); - auto angle1 = static_cast(angle_val[1]); - rotate_place_position_.push_back(angle0); - rotate_back_position_.push_back(angle1); - } - for (int i = 0; i < static_cast(rotate_place_position_.size()); ++i) - { - std::cout << "place i:" << i << " num:" << rotate_place_position_[i] << std::endl; - std::cout << "back i:" << i << " num:" << rotate_back_position_[i] << std::endl; - } } void Dart2Manual::gameRobotStatusCallback(const rm_msgs::GameRobotStatus::ConstPtr& data) { ManualBase::gameRobotStatusCallback(data); robot_id_ = data->robot_id; - if (!clamp_calibrate_) - { - clamp_calibration_->reset(); - clamp_calibrate_ = true; - } } void Dart2Manual::remoteControlTurnOn() @@ -174,6 +178,59 @@ void Dart2Manual::remoteControlTurnOn() gimbal_calibration_->reset(); shooter_calibration_->stopController(); shooter_calibration_->reset(); + clamp_calibration_->stopController(); + clamp_calibration_->reset(); +} + +void Dart2Manual::checkReferee() +{ + ManualBase::checkReferee(); +} + +void Dart2Manual::run() +{ + ManualBase::run(); + gimbal_calibration_->update(ros::Time::now()); + shooter_calibration_->update(ros::Time::now()); + clamp_calibration_->update(ros::Time::now()); + updateLaunchMode(); + updateAutoAimState(); +} + +void Dart2Manual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ +} + +void Dart2Manual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) +{ + ManualBase::updateRc(dbus_data); + if (std::abs(dbus_data->ch_r_x) > std::abs(dbus_data->ch_r_y)) + { + move(yaw_sender_, dbus_data->ch_r_x); + } + if (std::abs(dbus_data->ch_r_y) > std::abs(dbus_data->ch_r_x)) + { + move(range_sender_, dbus_data->ch_r_y); + } + operateClamper(dbus_data); +} + +void Dart2Manual::move(rm_common::JointPointCommandSender* joint, double ch) +{ + if (!joint_state_.position.empty()) + { + double position = joint_state_.position[joint->getIndex()]; + if (ch != 0.) + { + joint->setPoint(position - ch * scale_); + if_stop_ = true; + } + if (ch == 0. && if_stop_) + { + joint->setPoint(joint_state_.position[joint->getIndex()]); + if_stop_ = false; + } + } } void Dart2Manual::updateLaunchMode() @@ -188,18 +245,6 @@ void Dart2Manual::updateLaunchMode() case PULLDOWN: pullDown(); break; - case ROTATE_PLACE: - rotatePlace(); - break; - case LOADING: - loading(); - break; - case ROTATE_BACK: - rotateBack(); - break; - case LOADED: - loaded(); - break; case ENGAGE: engage(); break; @@ -219,6 +264,7 @@ void Dart2Manual::updateLaunchMode() } last_launch_mode_ = launch_mode_; } + void Dart2Manual::init() { ROS_INFO("Enter INIT"); @@ -226,84 +272,102 @@ void Dart2Manual::init() belt_left_sender_->setPoint(0.0); belt_right_sender_->setPoint(0.0); trigger_sender_->setPoint(trigger_home_command_); - if (last_launch_mode_ == PUSH) - long_camera_x_after_push_ = long_camera_x_; - auto_aim_state_ = NONE; + load_sender_->setPoint(load_init_position_); + clamp_left_sender_->setPoint(0.0); + clamp_mid_sender_->setPoint(0.0); + clamp_right_sender_->setPoint(0.0); + if (dart_fired_num_ > 3) { - rotate_sender_->setPoint(rotate_back_position_[3]); dart_fired_num_ = 0; - need_back_ = true; + ROS_INFO("Dart_fired_num reset to zero!"); } last_init_time_ = ros::Time::now(); + auto_aim_state_ = NONE; + + if (dart_fired_num_ == 0) + { + camera_x_set_point_ = 0.0; + current_x_offset_ = 0.0; + camera_y_set_point_ = 0.0; + current_y_offset_ = 0.0; + camera_x_init_ = false; + } + + start_aim_ = false; + aim_failed_ = false; + auto_aim_start_ = false; + + load_start_ = false; + load_finished_ = false; + belt_left_ready_ = false; + belt_right_ready_ = false; + dart_ready_ = false; + vision_ready_ = false; + retarget = false; + clamp_finished_ = false; + + push_failed = false; + push_succeeded = false; + + if (!camera_x_init_) + { + camera_x_after_push_ = 0; + camera_x_init_ = true; + } } + void Dart2Manual::pullDown() { ROS_INFO("Enter PULLDOWN"); - rotate_sender_->setPoint(rotate_back_position_[dart_fired_num_]); trigger_sender_->setPoint(trigger_work_command_); - belt_left_sender_->setPoint(downward_vel_); - belt_right_sender_->setPoint(downward_vel_); -} -void Dart2Manual::rotatePlace() -{ - ROS_INFO("Enter ROTATE_PLACE"); - belt_left_sender_->setPoint(0.0); - belt_right_sender_->setPoint(0.0); - rotate_sender_->setPoint(rotate_place_position_[dart_fired_num_]); - confirm_place_ = true; - last_rotate_time_ = ros::Time::now(); + belt_left_sender_->setPoint(belt_left_downward_vel_); + belt_right_sender_->setPoint(belt_right_downward_vel_); } -void Dart2Manual::loading() +void Dart2Manual::engage() { - ROS_INFO("Enter LOADING"); - setGripperAction(dart_fired_num_); - confirm_place_ = false; - last_loading_time_ = ros::Time::now(); -} + ROS_INFO("Enter ENGAGE"); -void Dart2Manual::rotateBack() -{ - ROS_INFO("Enter ROTATE_BACK"); - belt_left_sender_->setPoint(0.0); - belt_right_sender_->setPoint(0.0); - rotate_sender_->setPoint(rotate_back_position_[dart_fired_num_]); - confirm_back_ = true; - last_rotate_time_ = ros::Time::now(); -} + size_t current_dart_idx = static_cast(dart_fired_num_ % 4); + bool is_dart_active = (trigger_status_.size() > current_dart_idx) ? trigger_status_[current_dart_idx] : true; -void Dart2Manual::loaded() -{ - ROS_INFO("Enter LOADED"); - belt_left_sender_->setPoint(downward_vel_); - belt_right_sender_->setPoint(downward_vel_); - confirm_back_ = false; -} + if (is_dart_active) + { + trigger_sender_->setPoint(trigger_home_command_); + } + else + { + trigger_sender_->setPoint(trigger_work_command_); + ROS_INFO("Dart %d is disabled, using work command.", dart_fired_num_); + } -void Dart2Manual::engage() -{ - ROS_INFO("Enter ENGAGE"); - trigger_sender_->setPoint(trigger_home_command_); - belt_left_sender_->setPoint(0.0); - belt_right_sender_->setPoint(0.0); + belt_left_sender_->setPoint(0.25); + belt_right_sender_->setPoint(0.25); last_engage_time_ = ros::Time::now(); + + if (camera_x_init_ && dart_fired_num_ != 0) + { + camera_x_after_push_ = camera_x_; + } } void Dart2Manual::pullUp() { - ROS_INFO("Enter PULLUP"); - rotate_sender_->setPoint(rotate_place_position_[0]); - belt_right_sender_->setPoint(upward_vel_); - belt_left_sender_->setPoint(upward_vel_); + belt_left_sender_->setPoint(belt_right_upward_vel_); + belt_right_sender_->setPoint(belt_left_upward_vel_); } + void Dart2Manual::ready() { ROS_INFO("Enter READY"); belt_right_sender_->setPoint(0.0); belt_left_sender_->setPoint(0.0); + trigger_sender_->setPoint(trigger_home_command_); last_ready_time_ = ros::Time::now(); + launch_result_ = false; } + void Dart2Manual::push() { ROS_INFO("Enter PUSH"); @@ -311,232 +375,188 @@ void Dart2Manual::push() dart_fired_num_++; ROS_INFO("Launch dart num:%d", dart_fired_num_); last_push_time_ = ros::Time::now(); - is_long_camera_aim_ = false; + load_finished_ = false; + if (triggerIsWorked()) + { + launch_result_ = true; + ROS_INFO_THROTTLE(2.0, "Launch Dart successed."); + } } -void Dart2Manual::updateAutoAimState() +void Dart2Manual::readyLaunchDart(int dart_fired_num) { - switch (auto_aim_state_) + switch (launch_mode_) { - case AIM: - aim(); + case INIT: + if (shooter_calibration_->isCalibrated() && ros::Time::now() - last_init_time_ > ros::Duration(0.2) && + last_init_time_ > last_push_time_) + { + launch_mode_ = PULLDOWN; + last_init_time_ = ros::Time::now(); + } break; - case ADJUST: - adjust(); + + case PULLDOWN: + if (belt_left_position_ >= belt_left_max_ && belt_right_position_ >= belt_right_max_ && + std::abs(load_velocity_) < 0.01) + { + launch_mode_ = ENGAGE; + last_engage_time_ = ros::Time::now(); + } + break; + + case ENGAGE: + { + size_t current_dart_idx = static_cast(dart_fired_num_ % 4); + bool is_dart_active = (trigger_status_.size() > current_dart_idx) ? trigger_status_[current_dart_idx] : true; + + if (is_dart_active) + { + if (triggerIsHome() && ros::Time::now() - last_engage_time_ > ros::Duration(1.0)) + { + launch_mode_ = PULLUP; + load_start_ = true; + auto_aim_start_ = true; + } + } + else + { + if (ros::Time::now() - last_engage_time_ > ros::Duration(false_engage_time_)) + { + launch_mode_ = PULLUP; + load_start_ = true; + auto_aim_start_ = true; + } + } + break; + } + + case PULLUP: + load_work(); + if (belt_left_position_ <= belt_left_slow_ && belt_left_position_ > belt_right_min_) + { + belt_left_sender_->setPoint(belt_left_upward_slow_vel_); + } + if (belt_right_position_ <= belt_right_slow_ && belt_right_position_ > belt_right_min_) + { + belt_right_sender_->setPoint(belt_right_upward_slow_vel_); + } + + if (belt_left_position_ <= belt_left_min_) + { + belt_left_sender_->setPoint(0.0); + belt_left_ready_ = true; + } + if (belt_right_position_ <= belt_right_min_) + { + belt_right_sender_->setPoint(0.0); + belt_right_ready_ = true; + } + + if (belt_left_ready_ && belt_right_ready_ && dart_ready_ && vision_ready_) + { + launch_mode_ = READY; + last_ready_time_ = ros::Time::now(); + } + break; + + default: break; } - last_auto_aim_state_ = auto_aim_state_; } -void Dart2Manual::aim() +void Dart2Manual::leftSwitchDownOn() { - if (last_auto_aim_state_ != auto_aim_state_) - { - ROS_INFO("ENTER AIM."); - start_aim_ = true; - start_aim_time_ = ros::Time::now(); - } - if (is_short_camera_found_ && !is_long_camera_found_) - short_camera_x_set_point_ += short_camera_x_ * short_camera_p_x_; - if (is_long_camera_found_) - { - long_camera_x_set_point_ += long_camera_x_ * long_camera_p_x_; - long_camera_y_set_point_ = long_camera_y_ * long_camera_p_y_; - } - if (std::abs(long_camera_x_) <= long_camera_x_threshold_ && long_camera_x_ != 0 && yaw_velocity_ < 0.001) - { - ROS_INFO("The %d dart had aimed.Now x error:%lf, yaw position: %lf ", dart_fired_num_, long_camera_x_, - joint_state_.position[yaw_sender_->getIndex()]); - start_aim_ = false; - auto_aim_state_ = AIMED; - } + launch_mode_ = INIT; } -void Dart2Manual::adjust() +void Dart2Manual::leftSwitchMidOn() { - if (last_auto_aim_state_ != auto_aim_state_) - { - ROS_INFO("ENTER ADJUST."); - last_adjust_time_ = ros::Time::now(); - } - if (dart_fired_num_ == 0 && !had_adjust_) + switch (manual_state_) { - long_camera_x_set_point_ += dart_list_[dart_fired_num_].base_offset_; - long_camera_y_set_point_ += dart_list_[dart_fired_num_].base_range_; - had_adjust_ = true; + case OUTPOST: + yaw_sender_->setPoint(yaw_outpost_ + camera_x_set_point_); + range_sender_->setPoint(range_outpost_ + camera_y_set_point_); + break; + case BASE: + yaw_sender_->setPoint(yaw_base_ + camera_x_set_point_); + range_sender_->setPoint(range_base_ + camera_y_set_point_); + break; } - else if (dart_fired_num_ != 0 && !had_adjust_) + readyLaunchDart(dart_fired_num_); + if (auto_aim_start_) { - long_camera_x_set_point_ += - (dart_list_[dart_fired_num_].base_offset_ - dart_list_[dart_fired_num_ - 1].base_offset_); - long_camera_y_set_point_ += (dart_list_[dart_fired_num_].base_range_ - dart_list_[dart_fired_num_ - 1].base_range_); - had_adjust_ = true; + autoAim(); } - if (yaw_velocity_ < 0.001 && ros::Time::now() - last_adjust_time_ > ros::Duration(0.1) && range_velocity_ < 0.001) +} + +void Dart2Manual::leftSwitchUpOn() +{ + // if (launch_mode_ == READY) { + if (launch_mode_ == READY && auto_aim_state_ == ADJUSTED && ros::Time::now() - vision_end_time_ > ros::Duration(0.5)) { - ROS_INFO("The %d dart had adjusted.Now x error:%lf, yaw position: %lf ,range position: %lf", dart_fired_num_, - long_camera_x_, joint_state_.position[yaw_sender_->getIndex()], - joint_state_.position[range_sender_->getIndex()]); - long_camera_x_before_push_ = long_camera_x_; - had_adjust_ = false; - auto_aim_state_ = ADJUSTED; + last_push_time_ = ros::Time::now(); + launch_mode_ = PUSH; } + + // if (launch_mode_ == PUSH && ros::Time::now() - last_push_time_ > ros::Duration(0.5) && !triggerIsWorked() && !push_failed + // && !push_succeeded) { + // trigger_sender_->setPoint(trigger_home_command_); + // push_failed_time = ros::Time::now(); + // push_failed = true; + // } + // if (ros::Time::now() - push_failed_time > ros::Duration(2.5) && push_failed && !triggerIsWorked() && !push_succeeded){ + // trigger_sender_->setPoint(trigger_work_command_); + // last_push_time_ = ros::Time::now(); + // push_failed = false; + // } + // + // if (triggerIsWorked()) { + // push_succeeded = true; + // } } -void Dart2Manual::autoAim() +void Dart2Manual::rightSwitchDownOn() { - camera_is_online_ = (ros::Time::now() - last_get_camera_data_time_ < ros::Duration(1.0)); - if (use_auto_aim_ && camera_is_online_) + recordPosition(dbus_data_); + if (dbus_data_.ch_l_y == 1.) { - if (start_aim_ && ros::Time::now() - start_aim_time_ > ros::Duration(3.0)) + if (manual_state_ == OUTPOST) { - start_aim_ = false; - ROS_INFO("Auto aim time out!!!"); - short_camera_x_set_point_ = 0; - long_camera_x_set_point_ = 0; - auto_aim_state_ = ADJUST; + yaw_sender_->setPoint(yaw_outpost_); + range_sender_->setPoint(range_outpost_); } - if (dart_fired_num_ == 0) - auto_aim_state_ = AIM; - if (dart_fired_num_ != 0) + else if (manual_state_ == BASE) { - if (abs(long_camera_x_after_push_ - long_camera_x_before_push_) < retarget_threshold_) - auto_aim_state_ = ADJUST; - else - auto_aim_state_ = AIM; + yaw_sender_->setPoint(yaw_base_); + range_sender_->setPoint(range_base_); + } + } + if (dbus_data_.ch_l_y == -1.) + { + if (manual_state_ == OUTPOST) + { + yaw_sender_->setPoint(target_position_["outpost"][0]); + range_sender_->setPoint(target_position_["outpost"][1]); + } + else if (manual_state_ == BASE) + { + yaw_sender_->setPoint(target_position_["base"][0]); + range_sender_->setPoint(target_position_["base"][1]); } - if (auto_aim_state_ == AIMED) - auto_aim_state_ = ADJUST; } - else if (auto_aim_state_ != ADJUSTED) - auto_aim_state_ = ADJUST; } -void Dart2Manual::run() +void Dart2Manual::rightSwitchMidRise() { - ManualBase::run(); - gimbal_calibration_->update(ros::Time::now()); - shooter_calibration_->update(ros::Time::now()); - clamp_calibration_->update(ros::Time::now()); - updateLaunchMode(); - updateAutoAimState(); - updateGripper(); - if (need_back_ && ros::Time::now() - last_init_time_ > ros::Duration(2.0)) - { - need_back_ = false; - rotate_sender_->setPoint(rotate_back_position_[1]); - } + ManualBase::rightSwitchMidRise(); + allow_dart_door_open_times_ = 0; + move_state_ = NORMAL; } -void Dart2Manual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) +void Dart2Manual::rightSwitchUpRise() { - ManualBase::updateRc(dbus_data); - if (std::abs(dbus_data->ch_r_x) > std::abs(dbus_data->ch_r_y)) - { - move(yaw_sender_, dbus_data->ch_r_x); - } - if (std::abs(dbus_data->ch_r_y) > std::abs(dbus_data->ch_r_x)) - { - move(range_sender_, dbus_data->ch_r_y); - } - operateGripper(dbus_data); -} - -void Dart2Manual::sendCommand(const ros::Time& time) -{ - belt_left_sender_->sendCommand(time); - belt_right_sender_->sendCommand(time); - range_sender_->sendCommand(time); - trigger_sender_->sendCommand(time); - yaw_sender_->sendCommand(time); - rotate_sender_->sendCommand(time); - clamp_left_sender_->sendCommand(time); - clamp_mid_sender_->sendCommand(time); - clamp_right_sender_->sendCommand(time); -} - -void Dart2Manual::checkReferee() -{ - ManualBase::checkReferee(); -} - -void Dart2Manual::move(rm_common::JointPointCommandSender* joint, double ch) -{ - if (!joint_state_.position.empty()) - { - double position = joint_state_.position[joint->getIndex()]; - if (ch != 0.) - { - joint->setPoint(position - ch * scale_); - if_stop_ = true; - } - if (ch == 0. && if_stop_) - { - joint->setPoint(joint_state_.position[joint->getIndex()]); - if_stop_ = false; - } - } -} - -void Dart2Manual::changeGripperState(rm_common::JointPointCommandSender* gripper, bool& is_release) -{ - if (is_release) - { - if (clamp_position_.find(gripper) != clamp_position_.end()) - gripper->setPoint(clamp_position_.at(gripper)); - is_release = false; - } - else - { - gripper->setPoint(release_position_); - is_release = true; - } -} - -void Dart2Manual::changeAllGripperState() -{ - changeGripperState(clamp_left_sender_, left_clamp_is_release_); - changeGripperState(clamp_mid_sender_, mid_clamp_is_release_); - changeGripperState(clamp_right_sender_, right_clamp_is_release_); -} - -void Dart2Manual::readyLaunchDart(int dart_fired_num) -{ - if (launch_mode_ == INIT && shooter_calibration_->isCalibrated() && - ros::Time::now() - last_init_time_ > ros::Duration(0.2) && last_init_time_ > last_push_time_) - launch_mode_ = PULLDOWN; - if (launch_mode_ == PULLDOWN && belt_left_position_ >= belt_left_place_ && - belt_right_position_ >= belt_right_place_ && dart_fired_num != 0) - launch_mode_ = ROTATE_PLACE; - if (launch_mode_ == ROTATE_PLACE && dart_fired_num != 0 && rotate_velocity_ < 0.01 && confirm_place_ && - ros::Time::now() - last_rotate_time_ > ros::Duration(1.0)) - launch_mode_ = LOADING; - // if (((launch_mode_ == LOADING && ros::Time::now() - last_loading_time_ > ros::Duration(0.6) && last_loading_time_ > - // last_rotate_time_) || (launch_mode_ == PULLDOWN && belt_left_position_ >= belt_left_place_)) && check_place_ ) - // launch_mode_ = PULLUP; - if (launch_mode_ == LOADING && dart_fired_num != 0 && !confirm_place_ && - ros::Time::now() - last_loading_time_ > ros::Duration(0.5)) - launch_mode_ = LOADED; - if ((launch_mode_ == LOADED || launch_mode_ == PULLDOWN) && belt_left_position_ >= belt_left_max_ && - belt_right_position_ >= belt_right_max_ && check_place_) - launch_mode_ = PULLUP; - if ((launch_mode_ == LOADED || launch_mode_ == PULLDOWN) && belt_left_position_ >= belt_left_max_ && - belt_right_position_ >= belt_right_max_ && !check_place_) - launch_mode_ = ENGAGE; - if (launch_mode_ == ENGAGE && triggerIsHome() && ros::Time::now() - last_engage_time_ > ros::Duration(0.3)) - launch_mode_ = PULLUP; - if (launch_mode_ == PULLUP && belt_left_position_ <= belt_left_min_ && belt_right_position_ <= belt_right_min_) - launch_mode_ = READY; -} - -void Dart2Manual::leftSwitchDownOn() -{ - launch_mode_ = INIT; -} - -bool Dart2Manual::triggerIsWorked() const -{ - return trigger_position_ >= trigger_confirm_work_; + ManualBase::rightSwitchUpRise(); } bool Dart2Manual::triggerIsHome() const @@ -544,158 +564,9 @@ bool Dart2Manual::triggerIsHome() const return trigger_position_ <= trigger_confirm_home_; } -void Dart2Manual::leftSwitchMidOn() -{ - switch (manual_state_) - { - case OUTPOST: - yaw_sender_->setPoint(yaw_outpost_ + long_camera_x_set_point_ + short_camera_x_set_point_); - range_sender_->setPoint(range_outpost_ + long_camera_y_set_point_); - break; - case BASE: - yaw_sender_->setPoint(yaw_base_ + long_camera_x_set_point_ + short_camera_x_set_point_); - range_sender_->setPoint(range_base_ + long_camera_y_set_point_); - break; - } - readyLaunchDart(dart_fired_num_); - if (launch_mode_ == READY) - { - autoAim(); - } - // autoAim(); -} - -void Dart2Manual::leftSwitchUpOn() -{ - if (launch_mode_ == READY && auto_aim_state_ == ADJUSTED) - launch_mode_ = PUSH; -} - -void Dart2Manual::rightSwitchMidRise() -{ - ManualBase::rightSwitchMidRise(); - allow_dart_door_open_times_ = 0; - move_state_ = NORMAL; -} - -void Dart2Manual::rightSwitchDownOn() -{ - recordPosition(dbus_data_); - if (dbus_data_.ch_l_y == 1.) - { - if (manual_state_ == OUTPOST) - { - yaw_sender_->setPoint(yaw_outpost_); - range_sender_->setPoint(range_outpost_); - } - else if (manual_state_ == BASE) - { - yaw_sender_->setPoint(yaw_base_); - range_sender_->setPoint(range_base_); - } - } - if (dbus_data_.ch_l_y == -1.) - { - if (manual_state_ == OUTPOST) - { - yaw_sender_->setPoint(target_position_["outpost"][0]); - range_sender_->setPoint(target_position_["outpost"][1]); - } - else if (manual_state_ == BASE) - { - yaw_sender_->setPoint(target_position_["base"][0]); - range_sender_->setPoint(target_position_["base"][1]); - } - } -} - -void Dart2Manual::rightSwitchUpRise() -{ - ManualBase::rightSwitchUpRise(); - launch_mode_ = INIT; -} - -void Dart2Manual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) +bool Dart2Manual::triggerIsWorked() const { - ManualBase::updatePc(dbus_data); - double velocity_threshold = 0.001; - if (range_velocity_ < velocity_threshold && yaw_velocity_ < velocity_threshold) - move_state_ = STOP; - else - move_state_ = MOVING; - if (referee_is_online_) - { - switch (auto_state_) - { - case OUTPOST: - // yaw_sender_->setPoint(yaw_outpost_ + dart_list_[dart_fired_num_].outpost_offset_); - // b_sender_->setPoint(b_outpost_); - yaw_sender_->setPoint(yaw_base_ + long_camera_x_set_point_ + short_camera_x_set_point_); - // range_sender_->setPoint(range_base_ + long_camera_y_set_point_); - break; - case BASE: - yaw_sender_->setPoint(yaw_base_ + long_camera_x_set_point_ + short_camera_x_set_point_); - // range_sender_->setPoint(range_base_ + long_camera_y_set_point_); - break; - } - // if (dart_launch_opening_status_ != 1 && game_progress_ != rm_msgs::GameStatus::IN_BATTLE) - // autoAim(); - if (game_progress_ == rm_msgs::GameStatus::IN_BATTLE) - { - if (last_dart_door_status_ - dart_launch_opening_status_ == - rm_msgs::DartClientCmd::CLOSED - rm_msgs::DartClientCmd::OPENING_OR_CLOSING) - { - allow_push_ = true; - has_fired_num_ = 0; - ROS_INFO("Door open.allow push:%d,allow_dart_door_open_times: %d ", allow_push_, allow_dart_door_open_times_); - } - if (last_dart_door_status_ - dart_launch_opening_status_ == - rm_msgs::DartClientCmd::OPENED - rm_msgs::DartClientCmd::OPENING_OR_CLOSING) - { - allow_push_ = false; - allow_dart_door_open_times_--; - ROS_INFO("door close.now allow dart_door open times: %d, allow push: %d", allow_dart_door_open_times_, - allow_push_); - } - if (allow_dart_door_open_times_ > 0) - { - if (allow_push_ && has_fired_num_ < 2) - { - range_sender_->setPoint(range_base_ + long_camera_y_set_point_); - readyLaunchDart(dart_fired_num_); - if (launch_mode_ == READY) - autoAim(); - switch (move_state_) - { - case MOVING: - break; - case STOP: - if (launch_mode_ == READY && ros::Time::now() - last_ready_time_ > ros::Duration(0.3) && - last_ready_time_ > last_push_time_ && auto_aim_state_ == ADJUSTED && - dart_launch_opening_status_ == rm_msgs::DartClientCmd::OPENED) - { - launch_mode_ = PUSH; - has_fired_num_++; - ROS_INFO("has fired_num_=%d", has_fired_num_); - } - if (launch_mode_ == PUSH && last_push_time_ > last_ready_time_ && - ros::Time::now() - last_push_time_ > ros::Duration(0.3)) - { - ROS_INFO("now time: %f; last push time: %f", ros::Time::now().toSec(), last_push_time_.toSec()); - launch_mode_ = INIT; - } - break; - } - } - if (last_push_time_ > last_ready_time_ && ros::Time::now() - last_push_time_ > ros::Duration(0.3) && - has_fired_num_ == 2) - launch_mode_ = INIT; - } - else - range_sender_->setPoint(0.01); - } - last_dart_door_status_ = dart_launch_opening_status_; - } + return trigger_position_ >= trigger_confirm_work_; } void Dart2Manual::recordPosition(const rm_msgs::DbusData dbus_data) @@ -717,6 +588,19 @@ void Dart2Manual::recordPosition(const rm_msgs::DbusData dbus_data) } } +void Dart2Manual::sendCommand(const ros::Time& time) +{ + belt_left_sender_->sendCommand(time); + belt_right_sender_->sendCommand(time); + range_sender_->sendCommand(time); + trigger_sender_->sendCommand(time); + yaw_sender_->sendCommand(time); + load_sender_->sendCommand(time); + clamp_left_sender_->sendCommand(time); + clamp_mid_sender_->sendCommand(time); + clamp_right_sender_->sendCommand(time); +} + void Dart2Manual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) { ManualBase::dbusDataCallback(data); @@ -727,59 +611,13 @@ void Dart2Manual::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) trigger_position_ = joint_state_.position[trigger_sender_->getIndex()]; yaw_velocity_ = std::abs(joint_state_.velocity[yaw_sender_->getIndex()]); range_velocity_ = std::abs(joint_state_.velocity[range_sender_->getIndex()]); - rotate_velocity_ = std::abs(joint_state_.velocity[rotate_sender_->getIndex()]); + load_velocity_ = std::abs(joint_state_.velocity[load_sender_->getIndex()]); } wheel_clockwise_event_.update(data->wheel == 1.0); wheel_anticlockwise_event_.update(data->wheel == -1.0); dbus_data_ = *data; } -void Dart2Manual::updateAllowDartDoorOpenTimes() -{ - int elapsed_time = 420 - remain_time_; - if (!triggered_30s_ && elapsed_time > 30) - { - allow_dart_door_open_times_++; - triggered_30s_ = true; - ROS_INFO("30s into the match. allow dart door open times: %d", allow_dart_door_open_times_); - } - if (!triggered_4min_ && elapsed_time > 240) - { - allow_dart_door_open_times_++; - triggered_4min_ = true; - if (launch_mode_ == PUSH) - launch_mode_ = INIT; - ROS_INFO("420s into the match. allow dart door open times: %d", allow_dart_door_open_times_); - } -} - -void Dart2Manual::updateGripper() -{ - if (last_gripper_state_ != gripper_state_) - { - switch (gripper_state_) - { - case LEFT: - ROS_INFO("Left gripper change state."); - changeGripperState(clamp_left_sender_, left_clamp_is_release_); - break; - case MID: - ROS_INFO("Mid gripper change state."); - changeGripperState(clamp_mid_sender_, mid_clamp_is_release_); - break; - case RIGHT: - ROS_INFO("Right gripper change state."); - changeGripperState(clamp_right_sender_, right_clamp_is_release_); - break; - case ALL: - ROS_INFO("All gripper change state."); - changeAllGripperState(); - break; - } - } - last_gripper_state_ = gripper_state_; -} - void Dart2Manual::gameStatusCallback(const rm_msgs::GameStatus::ConstPtr& data) { ManualBase::gameStatusCallback(data); @@ -788,7 +626,6 @@ void Dart2Manual::gameStatusCallback(const rm_msgs::GameStatus::ConstPtr& data) remain_time_ = data->stage_remain_time; else remain_time_ = 420; - updateAllowDartDoorOpenTimes(); } void Dart2Manual::dartClientCmdCallback(const rm_msgs::DartClientCmd::ConstPtr& data) @@ -798,108 +635,387 @@ void Dart2Manual::dartClientCmdCallback(const rm_msgs::DartClientCmd::ConstPtr& void Dart2Manual::gameRobotHpCallback(const rm_msgs::GameRobotHp::ConstPtr& data) { - switch (robot_id_) +} + +void Dart2Manual::wheelClockwise() +{ + switch (clamp_num_) { - case rm_msgs::GameRobotStatus::RED_DART: - outpost_hp_ = data->blue_outpost_hp; + case 0: + clamp_num_ = 1; + temp_load_position_ = load_init_position_; + break; + case 1: + clamp_num_ = 2; + temp_load_position_ = load_left_position_; + break; + case 2: + clamp_num_ = 3; + temp_load_position_ = load_mid_position_; break; - case rm_msgs::GameRobotStatus::BLUE_DART: - outpost_hp_ = data->red_outpost_hp; + case 3: + temp_load_position_ = load_right_position_; + clamp_num_ = 0; break; } - if (outpost_hp_ != 0) - auto_state_ = OUTPOST; - else - auto_state_ = BASE; -} + ROS_INFO("Now temp_load_position_: %lf", temp_load_position_); -void Dart2Manual::wheelClockwise() -{ switch (move_state_) { case NORMAL: scale_ = scale_micro_; move_state_ = MICRO; ROS_INFO("Pitch and yaw : MICRO_MOVE_MODE"); + temp_clamp_ = true; break; case MICRO: scale_ = 0.01; move_state_ = NORMAL; ROS_INFO("Pitch and yaw : NORMAL_MOVE_MODE"); + temp_clamp_ = false; break; } + + // dart_fired_num_ = dart_fired_num_ + 1; + // + // ROS_INFO("Changed launch dart num:%d", dart_fired_num_); } void Dart2Manual::wheelAntiClockwise() { - clamp_calibration_->reset(); + // if (temp_load_) + // { + // load_sender_->setPoint(0.04); + // temp_load_ = false; + // } + // else { + // load_sender_->setPoint(0.04); + // temp_load_ = true; + // } + + load_sender_->setPoint(temp_load_position_); + + // if (dart_fired_num_ != 0) { + // dart_fired_num_ = dart_fired_num_ - 1; + // } + + // ROS_INFO("Changed launch dart num:%d", dart_fired_num_); } +/* void Dart2Manual::longCameraDataCallback(const rm_msgs::Dart::ConstPtr& data) { is_long_camera_found_ = data->is_found; long_camera_x_ = data->distance; long_camera_y_ = data->height; last_get_camera_data_time_ = data->stamp; - // ROS_INFO("time error:%f",(ros::Time::now() - data->stamp).toSec()); } +*/ void Dart2Manual::shortCameraDataCallback(const rm_msgs::Dart::ConstPtr& data) { - is_short_camera_found_ = data->is_found; - short_camera_x_ = data->distance; - short_camera_y_ = data->height; - // last_get_camera_data_time_ = data->stamp; + is_camera_found_ = data->is_found; + camera_x_ = data->distance; + camera_y_ = data->height; + last_get_camera_data_time_ = data->stamp; +} + +void Dart2Manual::updateAutoAimState() +{ + switch (auto_aim_state_) + { + case AIM: + aim(); + break; + case ADJUST: + adjust(); + break; + } + last_auto_aim_state_ = auto_aim_state_; +} + +void Dart2Manual::aim() +{ + if (last_auto_aim_state_ != auto_aim_state_) + { + ROS_INFO("ENTER AIM."); + start_aim_ = true; + start_aim_time_ = ros::Time::now(); + } + if (is_camera_found_ && std::abs(camera_x_) >= camera_fast_x_threshold_) + { + camera_x_set_point_ += camera_x_ * camera_fast_p_x_; + } + if (is_camera_found_ && std::abs(camera_x_) < camera_fast_x_threshold_ && + std::abs(camera_x_) >= camera_normal_x_threshold_) + { + camera_x_set_point_ += camera_x_ * camera_normal_p_x_; + } + if (is_camera_found_ && std::abs(camera_x_) < camera_normal_x_threshold_ && + std::abs(camera_x_) >= camera_slow_x_threshold_) + { + if (retarget) + { + camera_x_set_point_ += camera_x_ * camera_retarget_slow_p_x_; + } + else + { + camera_x_set_point_ += camera_x_ * camera_slow_p_x_; + } + } + + if (auto_aim_state_ == AIM && std::abs(camera_x_) <= camera_slow_x_threshold_ && is_camera_found_ && + yaw_velocity_ < 0.01) + { + ROS_INFO("The %d dart had aimed.Now x error:%lf, yaw position: %lf ", dart_fired_num_, camera_x_, + joint_state_.position[yaw_sender_->getIndex()]); + ROS_INFO("aim time used: %lf s", (ros::Time::now() - start_aim_time_).toSec()); + start_aim_ = false; + auto_aim_state_ = AIMED; + } +} + +void Dart2Manual::adjust() +{ + if (last_auto_aim_state_ != auto_aim_state_) + { + ROS_INFO("ENTER ADJUST."); + last_adjust_time_ = ros::Time::now(); + } + + if (!had_adjust_) + { + target_x_offset = dart_list_[dart_fired_num_].base_offset_; + target_y_offset_ = dart_list_[dart_fired_num_].base_range_; + ROS_INFO("Adjusting Offset. Removing old: %lf, Adding new: %lf", current_x_offset_, target_x_offset); + + camera_x_set_point_ -= current_x_offset_; + camera_x_set_point_ += target_x_offset; + current_x_offset_ = target_x_offset; + + camera_y_set_point_ -= current_y_offset_; + camera_y_set_point_ += target_y_offset_; + current_y_offset_ = target_y_offset_; + + had_adjust_ = true; + } + + if (yaw_velocity_ < 0.001 && ros::Time::now() - last_adjust_time_ > ros::Duration(0.1) && range_velocity_ < 0.001) + { + ROS_INFO("The %d dart had adjusted.Now x error:%lf, yaw position: %lf ,range position: %lf", dart_fired_num_, + camera_x_, joint_state_.position[yaw_sender_->getIndex()], + joint_state_.position[range_sender_->getIndex()]); + camera_x_before_push_ = camera_x_; + had_adjust_ = false; + auto_aim_start_ = false; + auto_aim_state_ = ADJUSTED; + vision_ready_ = true; + vision_end_time_ = ros::Time::now(); + } +} + +void Dart2Manual::autoAim() +{ + camera_is_online_ = (ros::Time::now() - last_get_camera_data_time_ < ros::Duration(1.0)); + + if (use_auto_aim_ && camera_is_online_) + { + if (start_aim_ && ros::Time::now() - start_aim_time_ > ros::Duration(5.0)) + { + start_aim_ = false; + ROS_INFO("Auto aim time out!!!"); + aim_failed_ = true; + camera_x_set_point_ = 0; + current_x_offset_ = 0.0; + camera_y_set_point_ = 0; + current_y_offset_ = 0.0; + + auto_aim_state_ = ADJUST; + } + + if (!aim_failed_) + { + bool force_aim = (dart_fired_num_ == 0) || random_fixed_target_; + if (random_fixed_target_) + { + ROS_INFO_THROTTLE(5.0, "Random fixed target is enabled, auto aim will be forced for the first dart."); + } + if (force_aim) + { + if (auto_aim_state_ != ADJUSTED && auto_aim_state_ != ADJUST && auto_aim_state_ != AIMED && + auto_aim_state_ != AIM) + { + current_x_offset_ = 0.0; + current_y_offset_ = 0.0; + auto_aim_state_ = AIM; + } + } + else + { + if (auto_aim_state_ != ADJUSTED && auto_aim_state_ != ADJUST && auto_aim_state_ != AIMED && + auto_aim_state_ != AIM) + { + if (std::abs(camera_x_after_push_ - camera_x_before_push_) < retarget_threshold_) + { + auto_aim_state_ = ADJUST; + } + else + { + retarget = true; + current_x_offset_ = 0.0; + current_y_offset_ = 0.0; + auto_aim_state_ = AIM; + } + } + } + } + + if (auto_aim_state_ == AIMED) + { + auto_aim_state_ = ADJUST; + } + } + else if (auto_aim_state_ != ADJUST && auto_aim_state_ != ADJUSTED) + { + ROS_INFO("use_auto_aim False or Camera offline, aim disabled."); + camera_x_set_point_ = 0; + current_x_offset_ = 0.0; + camera_y_set_point_ = 0; + current_y_offset_ = 0.0; + + auto_aim_state_ = ADJUST; + } } -void Dart2Manual::setGripperAction(int dart_fired_num) +void Dart2Manual::load_dart(int step_idx) { - switch (dart_fired_num) + switch (step_idx) { case 0: + current_load_target_ = load_init_position_; break; case 1: - clamp_left_sender_->setPoint(release_position_); - left_clamp_is_release_ = true; + current_load_target_ = load_left_position_; break; case 2: - clamp_mid_sender_->setPoint(release_position_); - clamp_left_sender_->setPoint(clamp_finish_position_); - mid_clamp_is_release_ = true; - left_clamp_is_release_ = false; + clamp_left_sender_->setPoint(clamp_left_release_position_); + current_load_target_ = load_mid_position_; break; case 3: - clamp_right_sender_->setPoint(release_position_); - clamp_mid_sender_->setPoint(clamp_finish_position_); - right_clamp_is_release_ = true; - mid_clamp_is_release_ = false; + clamp_left_sender_->setPoint(clamp_left_release_position_); + clamp_mid_sender_->setPoint(clamp_mid_release_position_); + current_load_target_ = load_right_position_; break; + default: + return; + } + + load_sender_->setPoint(current_load_target_); + ROS_INFO_THROTTLE(1.0, "Load Executing Step %d: Target Pos %.3f", step_idx, current_load_target_); +} + +void Dart2Manual::load_work() +{ + if (use_load_) + { + cur_pos = joint_state_.position[load_sender_->getIndex()]; + + if (!clamp_finished_ && !joint_state_.position.empty()) + { + load_dart(dart_fired_num_ % 4); + } + + if (dart_fired_num_ % 4 == 0 && std::abs(load_velocity_) < 0.001 && !joint_state_.position.empty()) + { + load_start_ = false; + clamp_finished_ = true; + load_finished_ = true; + dart_ready_ = true; + load_sender_->setPoint(load_init_position_); + } + + if (!joint_state_.position.empty()) + { + if (std::abs(cur_pos - current_load_target_) < 0.03 && std::abs(load_velocity_) < 0.001 && !load_finished_) + { + load_finish_time_ = ros::Time::now(); + load_finished_ = true; + load_start_ = false; + } + } + else + { + ROS_WARN_THROTTLE(1.0, "joint_state_.position IS EMPTY!!"); + load_start_ = false; + load_finished_ = true; + clamp_finished_ = true; + dart_ready_ = true; + } + + if (!clamp_finished_) + { + last_clamp_finished_time_ = ros::Time::now(); + } + + if (!clamp_finished_ && load_finished_ && ros::Time::now() - load_finish_time_ > ros::Duration(1.0)) + { + switch (dart_fired_num_ % 4) + { + case 1: + clamp_left_sender_->setPoint(clamp_left_release_position_); + break; + case 2: + clamp_mid_sender_->setPoint(clamp_mid_release_position_); + break; + case 3: + clamp_right_sender_->setPoint(clamp_right_release_position_); + break; + case 0: + break; + } + clamp_finished_ = true; + } + + if (clamp_finished_ && load_finished_ && (ros::Time::now() - last_clamp_finished_time_) > ros::Duration(1.2)) + { + load_sender_->setPoint(load_init_position_); + if (std::abs(cur_pos - load_init_position_) < 0.03) + { + dart_ready_ = true; + } + } + } + else + { + ROS_INFO_THROTTLE(2.0, "use_load is false, skip load process."); + dart_ready_ = true; } } -void Dart2Manual::operateGripper(const rm_msgs::DbusData::ConstPtr& dbus_data) +void Dart2Manual::operateClamper(const rm_msgs::DbusData::ConstPtr& dbus_data) { if (dbus_data->ch_l_y > 0.75) { - gripper_state_ = MID; + clamp_left_sender_->setPoint(0.0); + clamp_mid_sender_->setPoint(0.0); + clamp_right_sender_->setPoint(0.0); return; } if (dbus_data->ch_l_x > 0.75) { - gripper_state_ = RIGHT; + clamp_right_sender_->setPoint(clamp_right_release_position_); return; } if (dbus_data->ch_l_y < -0.75) { - gripper_state_ = ALL; + clamp_mid_sender_->setPoint(clamp_mid_release_position_); return; } if (dbus_data->ch_l_x < -0.75) { - gripper_state_ = LEFT; + clamp_left_sender_->setPoint(clamp_left_release_position_); return; } - gripper_state_ = ZERO; } } // namespace rm_manual diff --git a/src/dart_manual.cpp b/src/dart_manual.cpp index e5042089..1aab1121 100644 --- a/src/dart_manual.cpp +++ b/src/dart_manual.cpp @@ -375,19 +375,6 @@ void DartManual::dartClientCmdCallback(const rm_msgs::DartClientCmd::ConstPtr& d void DartManual::gameRobotHpCallback(const rm_msgs::GameRobotHp::ConstPtr& data) { - switch (robot_id_) - { - case rm_msgs::GameRobotStatus::RED_DART: - outpost_hp_ = data->blue_outpost_hp; - break; - case rm_msgs::GameRobotStatus::BLUE_DART: - outpost_hp_ = data->red_outpost_hp; - break; - } - if (outpost_hp_ != 0) - auto_state_ = OUTPOST; - else - auto_state_ = BASE; } void DartManual::wheelClockwise()