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_;