Skip to content

Commit

Permalink
Merge pull request autowarefoundation#71 from tier4/chore/cherry-pick…
Browse files Browse the repository at this point in the history 
…-20220622

chore: cherry pick 2022-06-22
  • Loading branch information
@0x126
0x126 authored Jun 22, 2022
2 parents 5d584bb + fc3cd55 commit a2d896e
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 12 deletions.
61 changes: 54 additions & 7 deletions common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,10 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
pub_velocity_limit_input_->setSingleStep(5.0);
connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit()));

// Emergency Button
emergency_button_ptr_ = new QPushButton("Set Emergency");
connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton()));

// Layout
auto * v_layout = new QVBoxLayout;
auto * gate_mode_path_change_approval_layout = new QHBoxLayout;
Expand All @@ -114,6 +118,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
velocity_limit_layout->addWidget(velocity_limit_button_ptr_);
velocity_limit_layout->addWidget(pub_velocity_limit_input_);
velocity_limit_layout->addWidget(new QLabel(" [km/h]"));
velocity_limit_layout->addWidget(emergency_button_ptr_);
v_layout->addLayout(velocity_limit_layout);
setLayout(v_layout);
}
Expand All @@ -137,22 +142,28 @@ void AutowareStatePanel::onInitialize()
sub_gear_ = raw_node_->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
"/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1));

sub_engage_ = raw_node_->create_subscription<autoware_auto_vehicle_msgs::msg::Engage>(
"/api/autoware/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1));
sub_engage_ = raw_node_->create_subscription<tier4_external_api_msgs::msg::EngageStatus>(
"/api/external/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1));

sub_emergency_ = raw_node_->create_subscription<tier4_external_api_msgs::msg::Emergency>(
"/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1));

client_engage_ = raw_node_->create_client<tier4_external_api_msgs::srv::Engage>(
"/api/autoware/set/engage", rmw_qos_profile_services_default);
"/api/external/set/engage", rmw_qos_profile_services_default);

client_emergency_stop_ = raw_node_->create_client<tier4_external_api_msgs::srv::SetEmergency>(
"/api/autoware/set/emergency", rmw_qos_profile_services_default);

pub_velocity_limit_ = raw_node_->create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
"/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1));
"/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local());

pub_gate_mode_ = raw_node_->create_publisher<tier4_control_msgs::msg::GateMode>(
"/control/gate_mode_cmd", rclcpp::QoS(1));
"/control/gate_mode_cmd", rclcpp::QoS{1}.transient_local());

pub_path_change_approval_ = raw_node_->create_publisher<tier4_planning_msgs::msg::Approval>(
"/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/"
"path_change_approval",
rclcpp::QoS(1));
rclcpp::QoS{1}.transient_local());
}

void AutowareStatePanel::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg)
Expand Down Expand Up @@ -248,12 +259,25 @@ void AutowareStatePanel::onShift(
}

void AutowareStatePanel::onEngageStatus(
const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg)
const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg)
{
current_engage_ = msg->engage;
engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(current_engage_)));
}

void AutowareStatePanel::onEmergencyStatus(
const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg)
{
current_emergency_ = msg->emergency;
if (msg->emergency) {
emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency"));
emergency_button_ptr_->setStyleSheet("background-color: #FF0000;");
} else {
emergency_button_ptr_->setText(QString::fromStdString("Set Emergency"));
emergency_button_ptr_->setStyleSheet("background-color: #00FF00;");
}
}

void AutowareStatePanel::onClickVelocityLimit()
{
auto velocity_limit = std::make_shared<tier4_planning_msgs::msg::VelocityLimit>();
Expand Down Expand Up @@ -281,6 +305,29 @@ void AutowareStatePanel::onClickAutowareEngage()
});
}

void AutowareStatePanel::onClickEmergencyButton()
{
auto request = std::make_shared<tier4_external_api_msgs::srv::SetEmergency::Request>();
if (current_emergency_) {
request->emergency = false;
} else {
request->emergency = true;
}
RCLCPP_INFO(raw_node_->get_logger(), request->emergency ? "Set Emergency" : "Clear Emergency");

client_emergency_stop_->async_send_request(
request,
[this]([[maybe_unused]] rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedFuture
result) {
auto response = result.get();
if (response->status.code == tier4_external_api_msgs::msg::ResponseStatus::SUCCESS) {
RCLCPP_INFO(raw_node_->get_logger(), "service succeeded");
} else {
RCLCPP_WARN(
raw_node_->get_logger(), "service failed: %s", response->status.message.c_str());
}
});
}
void AutowareStatePanel::onClickGateMode()
{
const auto data = gate_mode_label_ptr_->text().toStdString() == "AUTO"
Expand Down
14 changes: 11 additions & 3 deletions common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,13 @@
#include <rviz_common/panel.hpp>

#include <autoware_auto_system_msgs/msg/autoware_state.hpp>
#include <autoware_auto_vehicle_msgs/msg/engage.hpp>
#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
#include <tier4_control_msgs/msg/external_command_selector_mode.hpp>
#include <tier4_control_msgs/msg/gate_mode.hpp>
#include <tier4_external_api_msgs/msg/emergency.hpp>
#include <tier4_external_api_msgs/msg/engage_status.hpp>
#include <tier4_external_api_msgs/srv/engage.hpp>
#include <tier4_external_api_msgs/srv/set_emergency.hpp>
#include <tier4_planning_msgs/msg/approval.hpp>
#include <tier4_planning_msgs/msg/velocity_limit.hpp>

Expand All @@ -47,14 +49,16 @@ public Q_SLOTS:
void onClickVelocityLimit();
void onClickGateMode();
void onClickPathChangeApproval();
void onClickEmergencyButton();

protected:
void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
void onSelectorMode(
const tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr msg);
void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg);
void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
void onEngageStatus(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg);
void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg);
void onEngageStatus(const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg);

rclcpp::Node::SharedPtr raw_node_;
rclcpp::Subscription<tier4_control_msgs::msg::GateMode>::SharedPtr sub_gate_mode_;
Expand All @@ -63,9 +67,11 @@ public Q_SLOTS:
rclcpp::Subscription<autoware_auto_system_msgs::msg::AutowareState>::SharedPtr
sub_autoware_state_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr sub_gear_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::Engage>::SharedPtr sub_engage_;
rclcpp::Subscription<tier4_external_api_msgs::msg::EngageStatus>::SharedPtr sub_engage_;

rclcpp::Client<tier4_external_api_msgs::srv::Engage>::SharedPtr client_engage_;
rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedPtr client_emergency_stop_;
rclcpp::Subscription<tier4_external_api_msgs::msg::Emergency>::SharedPtr sub_emergency_;

rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
rclcpp::Publisher<tier4_control_msgs::msg::GateMode>::SharedPtr pub_gate_mode_;
Expand All @@ -81,8 +87,10 @@ public Q_SLOTS:
QPushButton * gate_mode_button_ptr_;
QPushButton * path_change_approval_button_ptr_;
QSpinBox * pub_velocity_limit_input_;
QPushButton * emergency_button_ptr_;

bool current_engage_;
bool current_emergency_;
};

} // namespace rviz_plugins
Expand Down
8 changes: 6 additions & 2 deletions system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,10 +141,14 @@ void EmergencyHandler::publishControlCommands()
pub_hazard_cmd_->publish(createHazardCmdMsg());

// Publish gear
if (param_.use_parking_after_stopped && isStopped()) {
{
GearCommand msg;
msg.stamp = stamp;
msg.command = GearCommand::PARK;
if (param_.use_parking_after_stopped && isStopped()) {
msg.command = GearCommand::PARK;
} else {
msg.command = GearCommand::DRIVE;
}
pub_gear_cmd_->publish(msg);
}

Expand Down

0 comments on commit a2d896e

Please sign in to comment.