diff --git a/.gitignore b/.gitignore
index 9886e1ed..34187790 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,5 @@
*.py[cod]
+
+# Visual Studio Code
+.vscode/
+*.code-workspace
diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml
index b6396858..a55a3602 100644
--- a/common/tier4_control_rviz_plugin/package.xml
+++ b/common/tier4_control_rviz_plugin/package.xml
@@ -5,6 +5,7 @@
0.1.0
The tier4_vehicle_rviz_plugin package
Taiki Tanaka
+ Akiro Harada
Apache License 2.0
ament_cmake_auto
diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp
index 7fd700b2..a3f3413e 100644
--- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp
+++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp
@@ -102,6 +102,13 @@ ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent
void ManualController::update()
{
if (!raw_node_) return;
+
+ const auto velocity = sub_velocity_->takeData();
+ const double current_velocity = velocity ? velocity->longitudinal_velocity : 0.0;
+
+ const auto accel = sub_accel_->takeData();
+ const double current_acceleration = accel ? accel->accel.accel.linear.x : 0.0;
+
Control control_cmd;
{
control_cmd.stamp = raw_node_->get_clock()->now();
@@ -114,18 +121,18 @@ void ManualController::update()
* a_des = k_const *(v - v_des) + a (k < 0 )
*/
const double k = -0.5;
- const double v = current_velocity_;
+ const double v = current_velocity;
const double v_des = cruise_velocity_;
- const double a = current_acceleration_;
+ const double a = current_acceleration;
const double a_des = k * (v - v_des) + a;
control_cmd.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0);
}
GearCommand gear_cmd;
{
const double eps = 0.001;
- if (control_cmd.longitudinal.velocity > eps && current_velocity_ > -eps) {
+ if (control_cmd.longitudinal.velocity > eps && current_velocity > -eps) {
gear_cmd.command = GearCommand::DRIVE;
- } else if (control_cmd.longitudinal.velocity < -eps && current_velocity_ < eps) {
+ } else if (control_cmd.longitudinal.velocity < -eps && current_velocity < eps) {
gear_cmd.command = GearCommand::REVERSE;
control_cmd.longitudinal.acceleration *= -1.0;
} else {
@@ -157,8 +164,12 @@ void ManualController::onInitialize()
sub_gate_mode_ = raw_node_->create_subscription(
"/control/current_gate_mode", 10, std::bind(&ManualController::onGateMode, this, _1));
- sub_velocity_ = raw_node_->create_subscription(
- "/vehicle/status/velocity_status", 1, std::bind(&ManualController::onVelocity, this, _1));
+ sub_velocity_ =
+ autoware::universe_utils::InterProcessPollingSubscriber::create_subscription(
+ raw_node_.get(), "/vehicle/status/velocity_status", 1);
+
+ sub_accel_ = autoware::universe_utils::InterProcessPollingSubscriber::
+ create_subscription(raw_node_.get(), "/localization/acceleration", 1);
sub_engage_ = raw_node_->create_subscription(
"/api/autoware/get/engage", 10, std::bind(&ManualController::onEngageStatus, this, _1));
@@ -207,16 +218,6 @@ void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg)
}
}
-void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg)
-{
- current_velocity_ = msg->longitudinal_velocity;
-}
-
-void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg)
-{
- current_acceleration_ = msg->accel.accel.linear.x;
-}
-
void ManualController::onGear(const GearReport::ConstSharedPtr msg)
{
switch (msg->report) {
diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp
index 61b4f2a0..74806354 100644
--- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp
+++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp
@@ -21,6 +21,7 @@
#include
#include
#include
+#include
#include
#include
@@ -68,13 +69,13 @@ public Q_SLOTS: // NOLINT for Qt
void onTimer();
void onPublishControlCommand();
void onGateMode(const GateMode::ConstSharedPtr msg);
- void onVelocity(const VelocityReport::ConstSharedPtr msg);
- void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg);
void onEngageStatus(const Engage::ConstSharedPtr msg);
void onGear(const GearReport::ConstSharedPtr msg);
rclcpp::Node::SharedPtr raw_node_;
rclcpp::Subscription::SharedPtr sub_gate_mode_;
- rclcpp::Subscription::SharedPtr sub_velocity_;
+ autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr sub_velocity_;
+ autoware::universe_utils::InterProcessPollingSubscriber::SharedPtr
+ sub_accel_;
rclcpp::Subscription::SharedPtr sub_engage_;
rclcpp::Publisher::SharedPtr pub_gate_mode_;
rclcpp::Publisher::SharedPtr pub_control_command_;
@@ -84,8 +85,6 @@ public Q_SLOTS: // NOLINT for Qt
double cruise_velocity_{0.0};
double steering_angle_{0.0};
- double current_velocity_{0.0};
- double current_acceleration_{0.0};
QLabel * gate_mode_label_ptr_;
QLabel * gear_label_ptr_;