From c2c7b16dc81d7035dc89349e7a859ccc1cda4c9a Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 13 Jun 2022 12:18:15 +0900 Subject: [PATCH 1/5] revert: engage button action in autoware_state_panel (#1079) * Revert "fix(autoware_state_panel): fix message type for /api/autoware/get/engage (#666)" This reverts commit 49cc906418b15994b7facb881f3c133a9d8eb3a1. * Revert "fix(tier4_state_rviz_plugin): change service and topic name for engage (#633)" This reverts commit 15f43bc7063809d38c369e405a82d9666826c052. --- .../tier4_state_rviz_plugin/src/autoware_state_panel.cpp | 8 ++++---- .../tier4_state_rviz_plugin/src/autoware_state_panel.hpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 33177729e8359..3a588d857ff15 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -137,11 +137,11 @@ void AutowareStatePanel::onInitialize() sub_gear_ = raw_node_->create_subscription( "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); - sub_engage_ = raw_node_->create_subscription( - "/api/autoware/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1)); + sub_engage_ = raw_node_->create_subscription( + "/api/external/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1)); client_engage_ = raw_node_->create_client( - "/api/autoware/set/engage", rmw_qos_profile_services_default); + "/api/external/set/engage", rmw_qos_profile_services_default); pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1)); @@ -248,7 +248,7 @@ void AutowareStatePanel::onShift( } void AutowareStatePanel::onEngageStatus( - const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg) + const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg) { current_engage_ = msg->engage; engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(current_engage_))); diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index a53bea54452fc..ecc3f343715d8 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -24,10 +24,10 @@ #include #include -#include #include #include #include +#include #include #include #include @@ -54,7 +54,7 @@ public Q_SLOTS: const tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr msg); void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg); void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); - void onEngageStatus(const autoware_auto_vehicle_msgs::msg::Engage::ConstSharedPtr msg); + void onEngageStatus(const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::Subscription::SharedPtr sub_gate_mode_; @@ -63,7 +63,7 @@ public Q_SLOTS: rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_gear_; - rclcpp::Subscription::SharedPtr sub_engage_; + rclcpp::Subscription::SharedPtr sub_engage_; rclcpp::Client::SharedPtr client_engage_; From f807914b60e70fb1ad4a50bdd942bd2330518fba Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Mon, 13 Jun 2022 15:18:13 +0900 Subject: [PATCH 2/5] feat(tier4_state_rviz_plugin): add emergency button (#1048) * feat(tier4_state_rviz_plugin):add emergency button Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix * chore: add default button name Signed-off-by: Shumpei Wakabayashi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/autoware_state_panel.cpp | 47 +++++++++++++++++++ .../src/autoware_state_panel.hpp | 8 ++++ 2 files changed, 55 insertions(+) diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 3a588d857ff15..8b0903e55f343 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -97,6 +97,10 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa pub_velocity_limit_input_->setSingleStep(5.0); connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); + // Emergency Button + emergency_button_ptr_ = new QPushButton("Set Emergency"); + connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + // Layout auto * v_layout = new QVBoxLayout; auto * gate_mode_path_change_approval_layout = new QHBoxLayout; @@ -114,6 +118,7 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa velocity_limit_layout->addWidget(velocity_limit_button_ptr_); velocity_limit_layout->addWidget(pub_velocity_limit_input_); velocity_limit_layout->addWidget(new QLabel(" [km/h]")); + velocity_limit_layout->addWidget(emergency_button_ptr_); v_layout->addLayout(velocity_limit_layout); setLayout(v_layout); } @@ -140,9 +145,15 @@ void AutowareStatePanel::onInitialize() sub_engage_ = raw_node_->create_subscription( "/api/external/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1)); + sub_emergency_ = raw_node_->create_subscription( + "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1)); + client_engage_ = raw_node_->create_client( "/api/external/set/engage", rmw_qos_profile_services_default); + client_emergency_stop_ = raw_node_->create_client( + "/api/autoware/set/emergency", rmw_qos_profile_services_default); + pub_velocity_limit_ = raw_node_->create_publisher( "/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1)); @@ -254,6 +265,19 @@ void AutowareStatePanel::onEngageStatus( engage_status_label_ptr_->setText(QString::fromStdString(Bool2String(current_engage_))); } +void AutowareStatePanel::onEmergencyStatus( + const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg) +{ + current_emergency_ = msg->emergency; + if (msg->emergency) { + emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); + emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); + } else { + emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); + emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); + } +} + void AutowareStatePanel::onClickVelocityLimit() { auto velocity_limit = std::make_shared(); @@ -281,6 +305,29 @@ void AutowareStatePanel::onClickAutowareEngage() }); } +void AutowareStatePanel::onClickEmergencyButton() +{ + auto request = std::make_shared(); + if (current_emergency_) { + request->emergency = false; + } else { + request->emergency = true; + } + RCLCPP_INFO(raw_node_->get_logger(), request->emergency ? "Set Emergency" : "Clear Emergency"); + + client_emergency_stop_->async_send_request( + request, + [this]([[maybe_unused]] rclcpp::Client::SharedFuture + result) { + auto response = result.get(); + if (response->status.code == tier4_external_api_msgs::msg::ResponseStatus::SUCCESS) { + RCLCPP_INFO(raw_node_->get_logger(), "service succeeded"); + } else { + RCLCPP_WARN( + raw_node_->get_logger(), "service failed: %s", response->status.message.c_str()); + } + }); +} void AutowareStatePanel::onClickGateMode() { const auto data = gate_mode_label_ptr_->text().toStdString() == "AUTO" diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index ecc3f343715d8..1833d75e422ae 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -27,8 +27,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -47,6 +49,7 @@ public Q_SLOTS: void onClickVelocityLimit(); void onClickGateMode(); void onClickPathChangeApproval(); + void onClickEmergencyButton(); protected: void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg); @@ -54,6 +57,7 @@ public Q_SLOTS: const tier4_control_msgs::msg::ExternalCommandSelectorMode::ConstSharedPtr msg); void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg); void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); void onEngageStatus(const tier4_external_api_msgs::msg::EngageStatus::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; @@ -66,6 +70,8 @@ public Q_SLOTS: rclcpp::Subscription::SharedPtr sub_engage_; rclcpp::Client::SharedPtr client_engage_; + rclcpp::Client::SharedPtr client_emergency_stop_; + rclcpp::Subscription::SharedPtr sub_emergency_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; rclcpp::Publisher::SharedPtr pub_gate_mode_; @@ -81,8 +87,10 @@ public Q_SLOTS: QPushButton * gate_mode_button_ptr_; QPushButton * path_change_approval_button_ptr_; QSpinBox * pub_velocity_limit_input_; + QPushButton * emergency_button_ptr_; bool current_engage_; + bool current_emergency_; }; } // namespace rviz_plugins From fec161b23c451d661e25f2ab75f578d8303cf2d3 Mon Sep 17 00:00:00 2001 From: melike <41450930+meliketanrikulu@users.noreply.github.com> Date: Mon, 13 Jun 2022 14:14:06 +0300 Subject: [PATCH 3/5] fix(gnss_poser): add local cartesian option for origin value (#725) Signed-off-by: melike tanrikulu --- sensing/gnss_poser/CMakeLists.txt | 1 + .../config/set_local_origin.param.yaml | 11 ++++++++++ .../gnss_poser/include/gnss_poser/convert.hpp | 22 +++++++++++++++++++ .../include/gnss_poser/gnss_poser_core.hpp | 1 + .../include/gnss_poser/gnss_stat.hpp | 1 + .../gnss_poser/launch/gnss_poser.launch.xml | 4 +++- sensing/gnss_poser/src/gnss_poser_core.cpp | 6 +++++ 7 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 sensing/gnss_poser/config/set_local_origin.param.yaml diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt index bd9886d376cad..993cf2d9da9e1 100644 --- a/sensing/gnss_poser/CMakeLists.txt +++ b/sensing/gnss_poser/CMakeLists.txt @@ -37,4 +37,5 @@ rclcpp_components_register_node(gnss_poser_node ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/sensing/gnss_poser/config/set_local_origin.param.yaml new file mode 100644 index 0000000000000..878b8fa2b944e --- /dev/null +++ b/sensing/gnss_poser/config/set_local_origin.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + + # Latitude of local origin + latitude: 40.816617984672746 + + # Longitude of local origin + longitude: 29.360491808334285 + + # Altitude of local origin + altitude: 52.251157145314075 diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 9f5dc2b789e2d..517b871b52f0b 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -17,6 +17,7 @@ #include "gnss_poser/gnss_stat.hpp" #include +#include #include #include #include @@ -56,7 +57,28 @@ double EllipsoidHeight2OrthometricHeight( } return OrthometricHeight; } +GNSSStat NavSatFix2LocalCartesian( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, + sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger) +{ + GNSSStat local_cartesian; + local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN; + + try { + GeographicLib::LocalCartesian localCartesian_origin( + nav_sat_fix_origin_.latitude, nav_sat_fix_origin_.longitude, nav_sat_fix_origin_.altitude); + localCartesian_origin.Forward( + nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, + local_cartesian.x, local_cartesian.y, local_cartesian.z); + local_cartesian.latitude = nav_sat_fix_msg.latitude; + local_cartesian.longitude = nav_sat_fix_msg.longitude; + local_cartesian.altitude = nav_sat_fix_msg.altitude; + } catch (const GeographicLib::GeographicErr & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert NavSatFix to LocalCartesian" << err.what()); + } + return local_cartesian; +} GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger) { diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp index 1985c4aec7f47..872019d8e0353 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -90,6 +90,7 @@ class GNSSPoser : public rclcpp::Node std::string gnss_base_frame_; std::string map_frame_; + sensor_msgs::msg::NavSatFix nav_sat_fix_origin_; bool use_ublox_receiver_; int plane_zone_; diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp index fa145b5b2098a..20074be5ec48c 100644 --- a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -20,6 +20,7 @@ enum class CoordinateSystem { UTM = 0, MGRS = 1, PLANE = 2, + LOCAL_CARTESIAN = 3, }; struct GNSSStat diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml index cf4451915a89f..8b48d7801c85f 100755 --- a/sensing/gnss_poser/launch/gnss_poser.launch.xml +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -2,6 +2,7 @@ + @@ -12,7 +13,7 @@ - + @@ -34,5 +35,6 @@ + diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 41d45154c5181..c812e9579af16 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -36,6 +36,10 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); coordinate_system_ = static_cast(coordinate_system); + nav_sat_fix_origin_.latitude = declare_parameter("latitude", 0.0); + nav_sat_fix_origin_.longitude = declare_parameter("longitude", 0.0); + nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0); + int buff_epoch = declare_parameter("buff_epoch", 1); position_buffer_.set_capacity(buff_epoch); @@ -166,6 +170,8 @@ GNSSStat GNSSPoser::convert( gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger()); } else if (coordinate_system == CoordinateSystem::PLANE) { gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger()); + } else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN) { + gnss_stat = NavSatFix2LocalCartesian(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger()); } else { RCLCPP_ERROR_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), From 618c3e912a9a96efe86f205a9c498e0d57f6d5b7 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Mon, 13 Jun 2022 21:28:56 +0900 Subject: [PATCH 4/5] fix(tier4_state_rviz_plugin): qos (#1085) Signed-off-by: Shumpei Wakabayashi --- common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 8b0903e55f343..585ea34a9120d 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -155,15 +155,15 @@ void AutowareStatePanel::onInitialize() "/api/autoware/set/emergency", rmw_qos_profile_services_default); pub_velocity_limit_ = raw_node_->create_publisher( - "/planning/scenario_planning/max_velocity_default", rclcpp::QoS(1)); + "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); pub_gate_mode_ = raw_node_->create_publisher( - "/control/gate_mode_cmd", rclcpp::QoS(1)); + "/control/gate_mode_cmd", rclcpp::QoS{1}.transient_local()); pub_path_change_approval_ = raw_node_->create_publisher( "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/" "path_change_approval", - rclcpp::QoS(1)); + rclcpp::QoS{1}.transient_local()); } void AutowareStatePanel::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) From a2a8becdf56c7274384b2dcbecda96a9cfd1adb8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Jun 2022 08:19:00 +0900 Subject: [PATCH 5/5] ci(pre-commit): autoupdate (#1088) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/pre-commit/pre-commit-hooks: v4.2.0 → v4.3.0](https://github.com/pre-commit/pre-commit-hooks/compare/v4.2.0...v4.3.0) - [github.com/pre-commit/mirrors-clang-format: v14.0.1 → v14.0.4-1](https://github.com/pre-commit/mirrors-clang-format/compare/v14.0.1...v14.0.4-1) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index af5b6580d87e2..9fcde901e26da 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.2.0 + rev: v4.3.0 hooks: - id: check-json - id: check-merge-conflict @@ -67,7 +67,7 @@ repos: args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.1 + rev: v14.0.4-1 hooks: - id: clang-format types_or: [c++, c, cuda]