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

feat(rtc_manager_panel): add rtc all approval #2009

Merged
merged 9 commits into from
Oct 4, 2022
Merged

feat(rtc_manager_panel): add rtc all approval #2009

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
d25e65e
feat(rtc_manager_panel): add rtc all approval
taikitanaka3 Oct 4, 2022
979e2e9
chore: cosmetic change
taikitanaka3 Oct 4, 2022
05c677f
fix: static cast
taikitanaka3 Oct 4, 2022
789f6b6
chore: update text
taikitanaka3 Oct 4, 2022
08c4852
chore: update text
taikitanaka3 Oct 4, 2022
c3fc2cd
doc: update documents
taikitanaka3 Oct 4, 2022
2c92a01
Merge commit '08c4852c8e0c0325f405d8c5cd3180c442a713c0' into feature/…
taikitanaka3 Oct 4, 2022
4bfae05
doc: update
taikitanaka3 Oct 4, 2022
dbfaecd
doc: small update
taikitanaka3 Oct 4, 2022
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
109 changes: 53 additions & 56 deletions common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,44 +27,9 @@
namespace rviz_plugins
{
inline std::string Bool2String(const bool var) { return var ? "True" : "False"; }
inline bool uint2bool(uint8_t var) { return var == static_cast<uint8_t>(0) ? false : true; }
using std::placeholders::_1;
using std::placeholders::_2;
Module getModuleType(const std::string & module_name)
{
Module module;
if (module_name == "blind_spot") {
module.type = Module::BLIND_SPOT;
} else if (module_name == "crosswalk") {
module.type = Module::CROSSWALK;
} else if (module_name == "detection_area") {
module.type = Module::DETECTION_AREA;
} else if (module_name == "intersection") {
module.type = Module::INTERSECTION;
} else if (module_name == "no_stopping_area") {
module.type = Module::NO_STOPPING_AREA;
} else if (module_name == "occlusion_spot") {
module.type = Module::OCCLUSION_SPOT;
} else if (module_name == "stop_line") {
module.type = Module::NONE;
} else if (module_name == "traffic_light") {
module.type = Module::TRAFFIC_LIGHT;
} else if (module_name == "virtual_traffic_light") {
module.type = Module::TRAFFIC_LIGHT;
} else if (module_name == "lane_change_left") {
module.type = Module::LANE_CHANGE_LEFT;
} else if (module_name == "lane_change_right") {
module.type = Module::LANE_CHANGE_RIGHT;
} else if (module_name == "avoidance_left") {
module.type = Module::AVOIDANCE_LEFT;
} else if (module_name == "avoidance_right") {
module.type = Module::AVOIDANCE_RIGHT;
} else if (module_name == "pull_over") {
module.type = Module::PULL_OVER;
} else if (module_name == "pull_out") {
module.type = Module::PULL_OUT;
}
return module;
}

std::string getModuleName(const uint8_t module_type)
{
Expand Down Expand Up @@ -171,6 +136,20 @@ RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent)
}
v_layout->addWidget(auto_mode_table_);

// execution
auto * rtc_exe_layout = new QHBoxLayout;
{
exec_button_ptr_ = new QPushButton("exe mode");
taikitanaka3 marked this conversation as resolved.
Show resolved Hide resolved
exec_button_ptr_->setCheckable(false);
connect(exec_button_ptr_, &QPushButton::clicked, this, &RTCManagerPanel::onClickExecution);
rtc_exe_layout->addWidget(exec_button_ptr_);
wait_button_ptr_ = new QPushButton("wait mode");
taikitanaka3 marked this conversation as resolved.
Show resolved Hide resolved
wait_button_ptr_->setCheckable(false);
connect(wait_button_ptr_, &QPushButton::clicked, this, &RTCManagerPanel::onClickWait);
rtc_exe_layout->addWidget(wait_button_ptr_);
}
v_layout->addLayout(rtc_exe_layout);

// statuses
auto * rtc_table_layout = new QHBoxLayout;
{
Expand All @@ -194,9 +173,6 @@ void RTCManagerPanel::onInitialize()
{
raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();

sub_rtc_status_ = raw_node_->create_subscription<CooperateStatusArray>(
"/api/external/get/rtc_status", 1, std::bind(&RTCManagerPanel::onRTCStatus, this, _1));

client_rtc_commands_ = raw_node_->create_client<CooperateCommands>(
"/api/external/set/rtc_commands", rmw_qos_profile_services_default);

Expand All @@ -206,6 +182,9 @@ void RTCManagerPanel::onInitialize()
a->enable_auto_mode_cli = raw_node_->create_client<AutoMode>(
enable_auto_mode_namespace_ + "/" + a->module_name, rmw_qos_profile_services_default);
}

sub_rtc_status_ = raw_node_->create_subscription<CooperateStatusArray>(
"/api/external/get/rtc_status", 1, std::bind(&RTCManagerPanel::onRTCStatus, this, _1));
}

void RTCAutoMode::onChangeToAutoMode()
Expand All @@ -230,22 +209,41 @@ void RTCAutoMode::onChangeToManualMode()
auto_module_button_ptr->setChecked(false);
}

void RTCManagerPanel::onClickCommandRequest(const uint8_t command)
{
if (!cooperate_statuses_ptr_) return;
if (cooperate_statuses_ptr_->statuses.empty()) return;
auto executable_cooperate_commands_request = std::make_shared<CooperateCommands::Request>();
executable_cooperate_commands_request->stamp = cooperate_statuses_ptr_->stamp;
// send coop request
for (auto status : cooperate_statuses_ptr_->statuses) {
CooperateCommand cooperate_command;
cooperate_command.uuid = status.uuid;
cooperate_command.module = status.module;
cooperate_command.command.type = command;
executable_cooperate_commands_request->commands.emplace_back(cooperate_command);
}
client_rtc_commands_->async_send_request(executable_cooperate_commands_request);
}

void RTCManagerPanel::onClickExecution() { onClickCommandRequest(Command::ACTIVATE); }

void RTCManagerPanel::onClickWait() { onClickCommandRequest(Command::DEACTIVATE); }

void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg)
{
cooperate_statuses_ptr_ = std::make_shared<CooperateStatusArray>(*msg);
rtc_table_->clearContents();
if (msg->statuses.empty()) return;
std::vector<CooperateStatus> coop_vec;
std::copy(msg->statuses.cbegin(), msg->statuses.cend(), std::back_inserter(coop_vec));
std::sort(
coop_vec.begin(), coop_vec.end(), [](const CooperateStatus & c1, const CooperateStatus & c2) {
return c1.start_distance < c2.start_distance;
});
// this is to stable rtc display not to occupy too much
size_t min_display_size{5};
size_t max_display_size{10};
rtc_table_->setRowCount(std::max(min_display_size, std::min(coop_vec.size(), max_display_size)));
// rtc messages are already sorted by distance
rtc_table_->setRowCount(
std::max(min_display_size, std::min(msg->statuses.size(), max_display_size)));
int cnt = 0;
for (auto status : msg->statuses) {
if (static_cast<size_t>(cnt) > max_display_size) return;
// uuid
{
std::stringstream uuid;
Expand Down Expand Up @@ -276,7 +274,7 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg
}

// is operator safe
const bool is_execute = status.command_status.type;
const bool is_execute = uint2bool(status.command_status.type);
{
std::string text = is_execute ? "EXECUTE" : "WAIT";
if (status.auto_mode) text = "NONE";
Expand Down Expand Up @@ -313,16 +311,15 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg
label->setText(QString::fromStdString(finish_distance));
rtc_table_->setCellWidget(cnt, 6, label);
}
for (size_t i = 0; i < column_size_; i++) {
if (is_rtc_auto_mode || (is_aw_safe && is_execute)) {
rtc_table_->cellWidget(cnt, i)->setStyleSheet("background-color: #00FF00;");
} else if (is_aw_safe || is_execute) {
rtc_table_->cellWidget(cnt, i)->setStyleSheet("background-color: #FFFF00;");
} else {
rtc_table_->cellWidget(cnt, i)->setStyleSheet("background-color: #FF0000;");
}
}

// add color for recognition
if (is_rtc_auto_mode || (is_aw_safe && is_execute)) {
rtc_table_->cellWidget(cnt, 1)->setStyleSheet("background-color: #00FF00;");
} else if (is_aw_safe || is_execute) {
rtc_table_->cellWidget(cnt, 1)->setStyleSheet("background-color: #FFFF00;");
} else {
rtc_table_->cellWidget(cnt, 1)->setStyleSheet("background-color: #FF0000;");
}
cnt++;
}
}
Expand Down
12 changes: 11 additions & 1 deletion common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,10 @@ public Q_SLOTS:
class RTCManagerPanel : public rviz_common::Panel
{
Q_OBJECT
public Q_SLOTS:
void onClickExecution();
void onClickWait();

public:
explicit RTCManagerPanel(QWidget * parent = nullptr);
void onInitialize() override;
Expand All @@ -83,16 +87,22 @@ class RTCManagerPanel : public rviz_common::Panel
void onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg);
void onEnableService(
const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) const;
void onClickCommandRequest(const uint8_t command);

private:
rclcpp::Node::SharedPtr raw_node_;
rclcpp::Subscription<CooperateStatusArray>::SharedPtr sub_rtc_status_;
rclcpp::Client<CooperateCommands>::SharedPtr client_rtc_commands_;
rclcpp::Client<AutoMode>::SharedPtr enable_auto_mode_cli_;
std::vector<RTCAutoMode *> auto_modes_;
std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode";

std::shared_ptr<CooperateStatusArray> cooperate_statuses_ptr_;
QTableWidget * rtc_table_;
QTableWidget * auto_mode_table_;
QPushButton * exec_button_ptr_ = {nullptr};
QPushButton * wait_button_ptr_ = {nullptr};
size_t column_size_ = {7};
std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode";
};

} // namespace rviz_plugins
Expand Down