From e354ca8b7e4d553a5755045302cb85a93812bb19 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 14 Jun 2024 14:39:00 +0900 Subject: [PATCH 1/6] add vscode files to gitignore Signed-off-by: Autumn60 --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) 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 From 1cae802aae0c65bc376fc769821f911bd0838ae4 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 14 Jun 2024 14:41:36 +0900 Subject: [PATCH 2/6] replace rclcpp::Subscription to tier4_autoware_utils::InterProcessPollingSubscriber Signed-off-by: Autumn60 --- .../src/tools/manual_controller.cpp | 33 ++++++++++--------- .../src/tools/manual_controller.hpp | 9 +++-- 2 files changed, 21 insertions(+), 21 deletions(-) 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..0ecfffdd 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_ = + tier4_autoware_utils::InterProcessPollingSubscriber::create_subscription( + raw_node_.get(), "/vehicle/status/velocity_status", 1); + + sub_accel_ = tier4_autoware_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..19aa386b 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" @@ -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_; + tier4_autoware_utils::InterProcessPollingSubscriber::SharedPtr sub_velocity_; + tier4_autoware_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_; From f4dbf0987c23c1e2a358f3d8b4ee362258093eca Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Tue, 18 Jun 2024 09:45:14 +0900 Subject: [PATCH 3/6] add Akiro Harada as a maintainer in tier4_control_rviz_plugin/package.xml Signed-off-by: Autumn60 --- common/tier4_control_rviz_plugin/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml index bec78de0..4dbcef9e 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 @@ -29,4 +30,4 @@ ament_cmake - + \ No newline at end of file From c5b8ba73935f18f2f365ff4f14d89cf8f049da4d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 18 Jun 2024 00:45:41 +0000 Subject: [PATCH 4/6] style(pre-commit): autofix --- common/tier4_control_rviz_plugin/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/tier4_control_rviz_plugin/package.xml b/common/tier4_control_rviz_plugin/package.xml index 4dbcef9e..1c0619d2 100644 --- a/common/tier4_control_rviz_plugin/package.xml +++ b/common/tier4_control_rviz_plugin/package.xml @@ -30,4 +30,4 @@ ament_cmake - \ No newline at end of file + From 28d720ce170cb6deca995e768b08a7351723d631 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Wed, 19 Jun 2024 19:34:19 +0900 Subject: [PATCH 5/6] replace tier4_autoware_utils to autoware_universe_utils Signed-off-by: Autumn60 --- .../src/tools/manual_controller.cpp | 4 ++-- .../src/tools/manual_controller.hpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) 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 0ecfffdd..a5c44659 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -165,10 +165,10 @@ void ManualController::onInitialize() "/control/current_gate_mode", 10, std::bind(&ManualController::onGateMode, this, _1)); sub_velocity_ = - tier4_autoware_utils::InterProcessPollingSubscriber::create_subscription( + autoware_universe_utils::InterProcessPollingSubscriber::create_subscription( raw_node_.get(), "/vehicle/status/velocity_status", 1); - sub_accel_ = tier4_autoware_utils::InterProcessPollingSubscriber:: + sub_accel_ = autoware_universe_utils::InterProcessPollingSubscriber:: create_subscription(raw_node_.get(), "/localization/acceleration", 1); sub_engage_ = raw_node_->create_subscription( 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 19aa386b..8b1f948b 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp @@ -21,9 +21,9 @@ #include #include #include +#include #include #include -#include #include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" @@ -73,8 +73,8 @@ public Q_SLOTS: // NOLINT for Qt void onGear(const GearReport::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::Subscription::SharedPtr sub_gate_mode_; - tier4_autoware_utils::InterProcessPollingSubscriber::SharedPtr sub_velocity_; - tier4_autoware_utils::InterProcessPollingSubscriber::SharedPtr + 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_; From ff19c06bed4cec89192748148d23fe76b0256ed1 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Thu, 20 Jun 2024 13:31:00 +0900 Subject: [PATCH 6/6] fix namespace Signed-off-by: Autumn60 --- .../tier4_control_rviz_plugin/src/tools/manual_controller.cpp | 4 ++-- .../tier4_control_rviz_plugin/src/tools/manual_controller.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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 a5c44659..a3f3413e 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -165,10 +165,10 @@ void ManualController::onInitialize() "/control/current_gate_mode", 10, std::bind(&ManualController::onGateMode, this, _1)); sub_velocity_ = - autoware_universe_utils::InterProcessPollingSubscriber::create_subscription( + autoware::universe_utils::InterProcessPollingSubscriber::create_subscription( raw_node_.get(), "/vehicle/status/velocity_status", 1); - sub_accel_ = autoware_universe_utils::InterProcessPollingSubscriber:: + sub_accel_ = autoware::universe_utils::InterProcessPollingSubscriber:: create_subscription(raw_node_.get(), "/localization/acceleration", 1); sub_engage_ = raw_node_->create_subscription( 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 8b1f948b..74806354 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp @@ -73,8 +73,8 @@ public Q_SLOTS: // NOLINT for Qt void onGear(const GearReport::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::Subscription::SharedPtr sub_gate_mode_; - autoware_universe_utils::InterProcessPollingSubscriber::SharedPtr sub_velocity_; - autoware_universe_utils::InterProcessPollingSubscriber::SharedPtr + 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_;