Skip to content

Commit

Permalink
perf(vehicle_cmd_gate): reduce callback invocation
Browse files Browse the repository at this point in the history 
perf(vehicle_cmd_gate): reduce callback invocation

Signed-off-by: Takayuki AKAMINE <[email protected]>

perf(vehicle_cmd_gate): fixed some bugs

Signed-off-by: Takayuki AKAMINE <[email protected]>
  • Loading branch information
@takam5f2
takam5f2 committed Feb 28, 2024
1 parent aead86a commit 427cffb
Show file tree
Hide file tree
Showing 2 changed files with 142 additions and 24 deletions.
162 changes: 138 additions & 24 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,74 +81,108 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
create_publisher<MarkerArray>("~/is_filter_activated/marker_raw", durable_qos);
filter_activated_flag_pub_ =
create_publisher<BoolStamped>("~/is_filter_activated/flag", durable_qos);

// Unexecuted callback group
auto noexec_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
auto noexec_subscription_options = rclcpp::SubscriptionOptions();
noexec_subscription_options.callback_group = noexec_callback_group;

// Subscriber
external_emergency_stop_heartbeat_sub_ = create_subscription<Heartbeat>(
"input/external_emergency_stop_heartbeat", 1,
std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1));
gate_mode_sub_ = create_subscription<GateMode>(
"input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1));
"input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1), noexec_subscription_options);
engage_sub_ = create_subscription<EngageMsg>(
"input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1));
"input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1), noexec_subscription_options);
kinematics_sub_ = create_subscription<Odometry>(
"/localization/kinematic_state", 1,
[this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; });
"/localization/kinematic_state", 1, [this](Odometry::SharedPtr msg) {
assert(false);
current_kinematics_ = *msg;
}, noexec_subscription_options);
acc_sub_ = create_subscription<AccelWithCovarianceStamped>(
"input/acceleration", 1, [this](AccelWithCovarianceStamped::SharedPtr msg) {
assert(false);
current_acceleration_ = msg->accel.accel.linear.x;
});
steer_sub_ = create_subscription<SteeringReport>(
"input/steering", 1,
[this](SteeringReport::SharedPtr msg) { current_steer_ = msg->steering_tire_angle; });
}, noexec_subscription_options);
steer_sub_ =
create_subscription<SteeringReport>("input/steering", 1, [this](SteeringReport::SharedPtr msg) {
assert(false);
current_steer_ = msg->steering_tire_angle;
}, noexec_subscription_options);
// operation_mode_sub_ callback is executed as another thread.
operation_mode_sub_ = create_subscription<OperationModeState>(
"input/operation_mode", rclcpp::QoS(1).transient_local(),
[this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; });
[this](const OperationModeState::SharedPtr msg) {
assert(); // never called.
current_operation_mode_ = *msg;
}, noexec_subscription_options);
mrm_state_sub_ = create_subscription<MrmState>(
"input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1));
"input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1), noexec_subscription_options);

// Subscriber for auto
auto_control_cmd_sub_ = create_subscription<AckermannControlCommand>(
"input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1));

auto_turn_indicator_cmd_sub_ = create_subscription<TurnIndicatorsCommand>(
"input/auto/turn_indicators_cmd", 1,
[this](TurnIndicatorsCommand::ConstSharedPtr msg) { auto_commands_.turn_indicator = *msg; });
"input/auto/turn_indicators_cmd", 1, [this](TurnIndicatorsCommand::ConstSharedPtr msg) {
assert(false);
auto_commands_.turn_indicator = *msg;
}, noexec_subscription_options);

auto_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
"input/auto/hazard_lights_cmd", 1,
[this](HazardLightsCommand::ConstSharedPtr msg) { auto_commands_.hazard_light = *msg; });
"input/auto/hazard_lights_cmd", 1, [this](HazardLightsCommand::ConstSharedPtr msg) {
assert(false);
auto_commands_.hazard_light = *msg;
}, noexec_subscription_options);

auto_gear_cmd_sub_ = create_subscription<GearCommand>(
"input/auto/gear_cmd", 1,
[this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; });
"input/auto/gear_cmd", 1, [this](GearCommand::ConstSharedPtr msg) {
assert(false);
auto_commands_.gear = *msg;
}, noexec_subscription_options);

// Subscriber for external
remote_control_cmd_sub_ = create_subscription<AckermannControlCommand>(
"input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1));

remote_turn_indicator_cmd_sub_ = create_subscription<TurnIndicatorsCommand>(
"input/external/turn_indicators_cmd", 1,
[this](TurnIndicatorsCommand::ConstSharedPtr msg) { remote_commands_.turn_indicator = *msg; });
[this](TurnIndicatorsCommand::ConstSharedPtr msg) {
assert(false);
remote_commands_.turn_indicator = *msg;
},
noexec_subscription_options);

remote_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
"input/external/hazard_lights_cmd", 1,
[this](HazardLightsCommand::ConstSharedPtr msg) { remote_commands_.hazard_light = *msg; });
[this](HazardLightsCommand::ConstSharedPtr msg) {
assert(false);
remote_commands_.hazard_light = *msg;
},
noexec_subscription_options);

remote_gear_cmd_sub_ = create_subscription<GearCommand>(
"input/external/gear_cmd", 1,
[this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; });
"input/external/gear_cmd", 1, [this](GearCommand::ConstSharedPtr msg) {
assert(false);
remote_commands_.gear = *msg;
});

// Subscriber for emergency
emergency_control_cmd_sub_ = create_subscription<AckermannControlCommand>(
"input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1));

emergency_hazard_light_cmd_sub_ = create_subscription<HazardLightsCommand>(
"input/emergency/hazard_lights_cmd", 1,
[this](HazardLightsCommand::ConstSharedPtr msg) { emergency_commands_.hazard_light = *msg; });
"input/emergency/hazard_lights_cmd", 1, [this](HazardLightsCommand::ConstSharedPtr msg) {
assert(false);
emergency_commands_.hazard_light = *msg;
});

emergency_gear_cmd_sub_ = create_subscription<GearCommand>(
"input/emergency/gear_cmd", 1,
[this](GearCommand::ConstSharedPtr msg) { emergency_commands_.gear = *msg; });
"input/emergency/gear_cmd", 1, [this](GearCommand::ConstSharedPtr msg) {
assert(false);
emergency_commands_.gear = *msg;
});

// Parameter
use_emergency_handling_ = declare_parameter<bool>("use_emergency_handling");
Expand Down Expand Up @@ -280,6 +314,53 @@ bool VehicleCmdGate::isDataReady()
return true;
}

void VehicleCmdGate::updateSystemState() {
rclcpp::MessageInfo msg_info;

std::shared_ptr<void> gate_mode_type_erased_msg = gate_mode_sub_->create_message();

if (gate_mode_sub_->take_type_erased(gate_mode_type_erased_msg.get(), msg_info)) {
gate_mode_sub_->handle_message(gate_mode_type_erased_msg, msg_info);
}

EngageMsg::SharedPtr engage_msg = std::make_shared<EngageMsg>();

if (engage_sub_->take(*engage_msg, msg_info)) {
is_engaged_ = engage_msg->engage;
}

OperationModeState::SharedPtr operation_mode_msg = std::make_shared<OperationModeState>();

if (operation_mode_sub_->take(*operation_mode_msg, msg_info)) {
current_operation_mode_ = *operation_mode_msg;
}

std::shared_ptr<void> mrm_state_type_erased_msg = mrm_state_sub_->create_message();
if (mrm_state_sub_->take_type_erased(mrm_state_type_erased_msg.get(), msg_info)) {
mrm_state_sub_->handle_message(mrm_state_type_erased_msg, msg_info);
}

}

void VehicleCmdGate::updateVehicleStatus() {
rclcpp::MessageInfo msg_info;

Odometry::SharedPtr kinematics_msg = std::make_shared<Odometry>();
if (kinematics_sub_->take(*kinematics_msg, msg_info)) {
current_kinematics_ = *kinematics_msg;
}

AccelWithCovarianceStamped::SharedPtr acc_msg = std::make_shared<AccelWithCovarianceStamped>();
if (acc_sub_->take(*acc_msg, msg_info)) {
current_acceleration_ = acc_msg->accel.accel.linear.x;
}

SteeringReport::SharedPtr steer_msg = std::make_shared<SteeringReport>();
if (steer_sub_->take(*steer_msg, msg_info)) {
current_steer_ = steer_msg->steering_tire_angle;
}
}

// for auto
void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg)
{
Expand Down Expand Up @@ -312,6 +393,7 @@ void VehicleCmdGate::onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr

void VehicleCmdGate::onTimer()
{
updateSystemState();
updater_.force_update();

if (!isDataReady()) {
Expand Down Expand Up @@ -360,12 +442,33 @@ void VehicleCmdGate::onTimer()
TurnIndicatorsCommand turn_indicator;
HazardLightsCommand hazard_light;
GearCommand gear;
// Local variables for message
rclcpp::MessageInfo msg_info;
TurnIndicatorsCommand::SharedPtr turn_indicator_msg = std::make_shared<TurnIndicatorsCommand>();
HazardLightsCommand::SharedPtr hazard_light_msg = std::make_shared<HazardLightsCommand>();
GearCommand::SharedPtr gear_msg = std::make_shared<GearCommand>();

if (use_emergency_handling_ && is_system_emergency_) {
if (emergency_hazard_light_cmd_sub_->take(*hazard_light_msg, msg_info)) {
emergency_commands_.hazard_light = *hazard_light_msg;
}
if (emergency_gear_cmd_sub_->take(*gear_msg, msg_info)) {
emergency_commands_.gear = *gear_msg;
}
turn_indicator = emergency_commands_.turn_indicator;
hazard_light = emergency_commands_.hazard_light;
gear = emergency_commands_.gear;
} else {
if (current_gate_mode_.data == GateMode::AUTO) {
if (auto_turn_indicator_cmd_sub_->take(*turn_indicator_msg, msg_info)) {
auto_commands_.turn_indicator = *turn_indicator_msg;
}
if (auto_hazard_light_cmd_sub_->take(*hazard_light_msg, msg_info)) {
auto_commands_.hazard_light = *hazard_light_msg;
}
if (auto_gear_cmd_sub_->take(*gear_msg, msg_info)) {
auto_commands_.gear = *gear_msg;
}
turn_indicator = auto_commands_.turn_indicator;
hazard_light = auto_commands_.hazard_light;
gear = auto_commands_.gear;
Expand All @@ -376,6 +479,15 @@ void VehicleCmdGate::onTimer()
hazard_light.command = HazardLightsCommand::NO_COMMAND;
}
} else if (current_gate_mode_.data == GateMode::EXTERNAL) {
if (remote_turn_indicator_cmd_sub_->take(*turn_indicator_msg, msg_info)) {
remote_commands_.turn_indicator = *turn_indicator_msg;
}
if (remote_hazard_light_cmd_sub_->take(*hazard_light_msg, msg_info)) {
remote_commands_.hazard_light = *hazard_light_msg;
}
if (remote_gear_cmd_sub_->take(*gear_msg, msg_info)) {
remote_commands_.gear = *gear_msg;
}
turn_indicator = remote_commands_.turn_indicator;
hazard_light = remote_commands_.hazard_light;
gear = remote_commands_.gear;
Expand All @@ -392,6 +504,7 @@ void VehicleCmdGate::onTimer()

void VehicleCmdGate::publishControlCommands(const Commands & commands)
{
updateVehicleStatus();
// Check system emergency
if (use_emergency_handling_ && is_emergency_state_heartbeat_timeout_) {
return;
Expand Down Expand Up @@ -634,6 +747,7 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg)

void VehicleCmdGate::onEngage(EngageMsg::ConstSharedPtr msg)
{
assert(false); // never called.
is_engaged_ = msg->engage;
}

Expand Down
4 changes: 4 additions & 0 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,10 @@ class VehicleCmdGate : public rclcpp::Node
rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_; // for filter
rclcpp::Subscription<SteeringReport>::SharedPtr steer_sub_; // for filter

// function to take data from subscriptions
void updateSystemState();
void updateVehicleStatus();

void onGateMode(GateMode::ConstSharedPtr msg);
void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg);
void onMrmState(MrmState::ConstSharedPtr msg);
Expand Down

0 comments on commit 427cffb

Please sign in to comment.