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

feat(state_rviz_plugin): add GateMode and PathChangeApproval Button #894

Merged
merged 2 commits into from
May 13, 2022
Merged

feat(state_rviz_plugin): add GateMode and PathChangeApproval Button #894

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
81db3d2
feat(state_rviz_plugin): add GateMode and PathChangeApproval Button
h-ohta May 12, 2022
9de9769
ci(pre-commit): autofix
pre-commit-ci[bot] May 12, 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
40 changes: 40 additions & 0 deletions common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include "autoware_state_panel.hpp"

#include <QHBoxLayout>
#include <QString>
#include <QVBoxLayout>
#include <rviz_common/display_context.hpp>
Expand Down Expand Up @@ -80,14 +81,25 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
engage_button_ptr_ = new QPushButton("Engage");
connect(engage_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareEngage()));

// Gate Mode Button
gate_mode_button_ptr_ = new QPushButton("Gate Mode");
connect(gate_mode_button_ptr_, SIGNAL(clicked()), SLOT(onClickGateMode()));

// Path Change Approval Button
path_change_approval_button_ptr_ = new QPushButton("Path Change Approval");
connect(path_change_approval_button_ptr_, SIGNAL(clicked()), SLOT(onClickPathChangeApproval()));

// Velocity Limit
velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit");
pub_velocity_limit_input_ = new QSpinBox();
pub_velocity_limit_input_->setRange(-100.0, 100.0);
pub_velocity_limit_input_->setValue(0.0);
pub_velocity_limit_input_->setSingleStep(5.0);
connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit()));

// Layout
auto * v_layout = new QVBoxLayout;
auto * gate_mode_path_change_approval_layout = new QHBoxLayout;
auto * velocity_limit_layout = new QHBoxLayout();
v_layout->addLayout(gate_layout);
v_layout->addLayout(selector_layout);
Expand All @@ -96,6 +108,9 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa
v_layout->addLayout(engage_status_layout);
v_layout->addWidget(engage_button_ptr_);
v_layout->addLayout(engage_status_layout);
gate_mode_path_change_approval_layout->addWidget(gate_mode_button_ptr_);
gate_mode_path_change_approval_layout->addWidget(path_change_approval_button_ptr_);
v_layout->addLayout(gate_mode_path_change_approval_layout);
velocity_limit_layout->addWidget(velocity_limit_button_ptr_);
velocity_limit_layout->addWidget(pub_velocity_limit_input_);
velocity_limit_layout->addWidget(new QLabel(" [km/h]"));
Expand Down Expand Up @@ -130,6 +145,14 @@ void AutowareStatePanel::onInitialize()

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

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

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));
}

void AutowareStatePanel::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg)
Expand Down Expand Up @@ -258,6 +281,23 @@ void AutowareStatePanel::onClickAutowareEngage()
});
}

void AutowareStatePanel::onClickGateMode()
{
const auto data = gate_mode_label_ptr_->text().toStdString() == "AUTO"
? tier4_control_msgs::msg::GateMode::EXTERNAL
: tier4_control_msgs::msg::GateMode::AUTO;
RCLCPP_INFO(raw_node_->get_logger(), "data : %d", data);
pub_gate_mode_->publish(
tier4_control_msgs::build<tier4_control_msgs::msg::GateMode>().data(data));
}

void AutowareStatePanel::onClickPathChangeApproval()
{
pub_path_change_approval_->publish(
tier4_planning_msgs::build<tier4_planning_msgs::msg::Approval>()
.stamp(raw_node_->now())
.approval(true));
}
} // namespace rviz_plugins

#include <pluginlib/class_list_macros.hpp>
Expand Down
7 changes: 7 additions & 0 deletions common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include <tier4_control_msgs/msg/external_command_selector_mode.hpp>
#include <tier4_control_msgs/msg/gate_mode.hpp>
#include <tier4_external_api_msgs/srv/engage.hpp>
#include <tier4_planning_msgs/msg/approval.hpp>
#include <tier4_planning_msgs/msg/velocity_limit.hpp>

namespace rviz_plugins
Expand All @@ -44,6 +45,8 @@ class AutowareStatePanel : public rviz_common::Panel
public Q_SLOTS:
void onClickAutowareEngage();
void onClickVelocityLimit();
void onClickGateMode();
void onClickPathChangeApproval();

protected:
void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
Expand All @@ -65,6 +68,8 @@ public Q_SLOTS:
rclcpp::Client<tier4_external_api_msgs::srv::Engage>::SharedPtr client_engage_;

rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
rclcpp::Publisher<tier4_control_msgs::msg::GateMode>::SharedPtr pub_gate_mode_;
rclcpp::Publisher<tier4_planning_msgs::msg::Approval>::SharedPtr pub_path_change_approval_;

QLabel * gate_mode_label_ptr_;
QLabel * selector_mode_label_ptr_;
Expand All @@ -73,6 +78,8 @@ public Q_SLOTS:
QLabel * engage_status_label_ptr_;
QPushButton * engage_button_ptr_;
QPushButton * velocity_limit_button_ptr_;
QPushButton * gate_mode_button_ptr_;
QPushButton * path_change_approval_button_ptr_;
QSpinBox * pub_velocity_limit_input_;

bool current_engage_;
Expand Down