Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Modify the logic of switch and some names. #176

Merged
merged 10 commits into from
Jul 24, 2023
Merged

Modify the logic of switch and some names. #176

Changes from 9 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
f5e9f2f
Modify double barrel command sender.
Edwinlinks Jul 6, 2023
fa2af4c
Delete unused header.
Edwinlinks Jul 6, 2023
7dba350
Fix some logic bug about switching barrel.
NaHCO3bc Jul 9, 2023
2c544c2
Solve the bug that the frequency of shooter will not be set before th…
NaHCO3bc Jul 9, 2023
12bd9af
Add setMode
NaHCO3bc Jul 10, 2023
94ba2a1
Parameterize some thresholds.
NaHCO3bc Jul 12, 2023
95c9e88
Move the switchBarrel function into sendcommand.
NaHCO3bc Jul 12, 2023
6773ce0
Change the param name to is_switching and simplify the logic
NaHCO3bc Jul 12, 2023
085a426
Add two duration judgements to optimize the logic.
NaHCO3bc Jul 15, 2023
299d99e
Modify some names of params and add a frequency threshold
NaHCO3bc Jul 19, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
123 changes: 60 additions & 63 deletions rm_common/include/rm_common/decision/command_sender.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -699,8 +699,11 @@ class DoubleBarrelCommandSender
barrel_nh.getParam("is_double_barrel", is_double_barrel_);
barrel_nh.getParam("id1_point", id1_point_);
barrel_nh.getParam("id2_point", id2_point_);
barrel_nh.getParam("restart_push_threshold", restart_push_threshold_);
barrel_nh.getParam("cooling_threshold", cooling_threshold_);
barrel_nh.getParam("frequency_threshold", frequency_threshold_);
barrel_nh.getParam("check_launch_threshold", check_launch_threshold_);
barrel_nh.getParam("check_switch_threshold", check_switch_threshold_);
barrel_nh.getParam("ready_duration", ready_duration_);
barrel_nh.getParam("is_switching_duration", is_switching_duration_);

joint_state_sub_ = nh.subscribe<sensor_msgs::JointState>("/joint_states", 10,
&DoubleBarrelCommandSender::jointStateCallback, this);
Expand Down Expand Up @@ -741,46 +744,36 @@ class DoubleBarrelCommandSender

void setMode(int mode)
{
shooter_ID1_cmd_sender_->setMode(mode);
shooter_ID2_cmd_sender_->setMode(mode);
getBarrel()->setMode(mode);
}
void setZero()
{
shooter_ID1_cmd_sender_->setZero();
shooter_ID2_cmd_sender_->setZero();
getBarrel()->setZero();
}
void checkError(const ros::Time& time)
{
shooter_ID1_cmd_sender_->checkError(time);
shooter_ID2_cmd_sender_->checkError(time);
getBarrel()->checkError(time);
}
void sendCommand(const ros::Time& time)
{
if (doSwitch())
{
switch_barrel_ = true;
switch_done_ = false;
}
if ((shooter_ID1_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH ||
shooter_ID2_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH) &&
switch_barrel_)
{
setMode(rm_msgs::ShootCmd::READY);
if (!switch_done_)
switchBarrel();
else
doRestartPush();
}
shooter_ID1_cmd_sender_->sendCommand(time);
shooter_ID2_cmd_sender_->sendCommand(time);
if (checkSwitch())
need_switch_ = true;
if (need_switch_)
switchBarrel();
checklaunch();
if (getBarrel()->getMsg()->mode == rm_msgs::ShootCmd::PUSH)
last_push_time_ = time;
getBarrel()->sendCommand(time);
}
int initBarrelId()
void init()
{
ros::Time time = ros::Time::now();
barrel_id_ = 0;
barrel_command_sender_->setPoint(id1_point_);
shooter_ID1_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
shooter_ID2_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
barrel_command_sender_->sendCommand(time);
return barrel_id_;
shooter_ID1_cmd_sender_->sendCommand(time);
shooter_ID2_cmd_sender_->sendCommand(time);
}
void setArmorType(uint8_t armor_type)
{
Expand All @@ -789,73 +782,75 @@ class DoubleBarrelCommandSender
}
void setShootFrequency(uint8_t mode)
{
shooter_ID1_cmd_sender_->setShootFrequency(mode);
shooter_ID2_cmd_sender_->setShootFrequency(mode);
getBarrel()->setShootFrequency(mode);
}
uint8_t getShootFrequency()
{
return shooter_ID1_cmd_sender_->getShootFrequency();
return getBarrel()->getShootFrequency();
}
double getSpeed()
{
return shooter_ID1_cmd_sender_->getSpeed();
return getBarrel()->getSpeed();
}

private:
int getBarrelId()
ShooterCommandSender* getBarrel()
{
if (barrel_command_sender_->getMsg()->data == id1_point_)
barrel_id_ = 0;
is_id1_ = true;
else
barrel_id_ = 1;
return barrel_id_;
is_id1_ = false;
return is_id1_ ? shooter_ID1_cmd_sender_ : shooter_ID2_cmd_sender_;
}
void switchBarrel()
{
ros::Time time = ros::Time::now();
if (std::fmod(std::abs(trigger_error_), 2. * M_PI) < 0.01)
switch_done_ = true;
if (switch_done_)
bool time_to_switch = (std::fmod(std::abs(trigger_error_), 2. * M_PI) < check_switch_threshold_);
setMode(rm_msgs::ShootCmd::READY);
if (time_to_switch || (time - last_push_time_).toSec() > ready_duration_)
{
barrel_command_sender_->getMsg()->data == id2_point_ ? barrel_command_sender_->setPoint(id1_point_) :
barrel_command_sender_->setPoint(id2_point_);
barrel_command_sender_->sendCommand(time);
last_switch_time_ = time;
need_switch_ = false;
is_switching_ = true;
}
}
void doRestartPush()

void checklaunch()
{
if (std::abs(joint_state_.position[barrel_command_sender_->getIndex()] - barrel_command_sender_->getMsg()->data) <
restart_push_threshold_)
ros::Time time = ros::Time::now();
if (is_switching_)
{
switch_barrel_ = false;
setMode(rm_msgs::ShootCmd::PUSH);
setMode(rm_msgs::ShootCmd::READY);
if ((time - last_switch_time_).toSec() > is_switching_duration_ ||
(std::abs(joint_state_.position[barrel_command_sender_->getIndex()] -
barrel_command_sender_->getMsg()->data) < check_launch_threshold_))
is_switching_ = false;
}
}

bool doSwitch()
bool checkSwitch()
{
if (!is_double_barrel_)
return false;
int shooter_ID1_cooling_limit = shooter_ID1_cmd_sender_->heat_limit_->getCoolingLimit();
int shooter_ID2_cooling_limit = shooter_ID2_cmd_sender_->heat_limit_->getCoolingLimit();
int shooter_ID1_cooling_heat = shooter_ID1_cmd_sender_->heat_limit_->getCoolingHeat();
int shooter_ID2_cooling_heat = shooter_ID2_cmd_sender_->heat_limit_->getCoolingHeat();

if (shooter_ID1_cooling_limit == 0 || shooter_ID2_cooling_limit == 0)
if (shooter_ID1_cmd_sender_->heat_limit_->getCoolingLimit() == 0 ||
shooter_ID2_cmd_sender_->heat_limit_->getCoolingLimit() == 0)
{
ROS_WARN("Can not get cooling limit");
ROS_WARN_ONCE("Can not get cooling limit");
return false;
}

if (shooter_ID1_cooling_limit - shooter_ID1_cooling_heat < cooling_threshold_[0] ||
shooter_ID2_cooling_limit - shooter_ID2_cooling_heat < cooling_threshold_[0])
if (shooter_ID1_cmd_sender_->heat_limit_->getShootFrequency() == 0.0 ||
shooter_ID2_cmd_sender_->heat_limit_->getShootFrequency() == 0.0)
{
if (getBarrelId())
return shooter_ID2_cooling_limit - shooter_ID2_cooling_heat < cooling_threshold_[0] &&
shooter_ID1_cooling_limit - shooter_ID1_cooling_heat > cooling_threshold_[1];
if (getBarrel() == shooter_ID1_cmd_sender_)
return getBarrel()->heat_limit_->getShootFrequency() == 0.0 &&
shooter_ID2_cmd_sender_->heat_limit_->getShootFrequency() > frequency_threshold_;
else
return shooter_ID1_cooling_limit - shooter_ID1_cooling_heat < cooling_threshold_[0] &&
shooter_ID2_cooling_limit - shooter_ID2_cooling_heat > cooling_threshold_[1];
return getBarrel()->heat_limit_->getShootFrequency() == 0.0 &&
ye-luo-xi-tui marked this conversation as resolved.
Show resolved Hide resolved
shooter_ID1_cmd_sender_->heat_limit_->getShootFrequency() > frequency_threshold_;
}
else
return false;
Expand All @@ -874,12 +869,14 @@ class DoubleBarrelCommandSender
ros::Subscriber trigger_state_sub_;
ros::Subscriber joint_state_sub_;
sensor_msgs::JointState joint_state_;
bool is_double_barrel_{ false }, switch_barrel_{ false }, switch_done_{ true };
bool is_double_barrel_{ false }, need_switch_{ false }, is_switching_{ false };
ros::Time last_switch_time_, last_push_time_;
double ready_duration_, is_switching_duration_;
NaHCO3bc marked this conversation as resolved.
Show resolved Hide resolved
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

为啥叫ready_duration？

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

为啥叫ready_duration？

因为在这段时间里 shooter都是在ready中

double trigger_error_;
int barrel_id_;
bool is_id1_{ false };
double id1_point_, id2_point_;
double restart_push_threshold_;
std::vector<int> cooling_threshold_;
double frequency_threshold_;
double check_launch_threshold_, check_switch_threshold_;
};

} // namespace rm_common
Loading