Merge branch 'main' into use-absolute-value-in-lateraloffset-to-filte…

…r-on-Obstacles

.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]

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);
 }
@@ -137,22 +142,28 @@ void AutowareStatePanel::onInitialize()
   sub_gear_ = raw_node_->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
     "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1));
 
-  sub_engage_ = raw_node_->create_subscription<autoware_auto_vehicle_msgs::msg::Engage>(
-    "/api/autoware/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1));
+  sub_engage_ = raw_node_->create_subscription<tier4_external_api_msgs::msg::EngageStatus>(
+    "/api/external/get/engage", 10, std::bind(&AutowareStatePanel::onEngageStatus, this, _1));
+
+  sub_emergency_ = raw_node_->create_subscription<tier4_external_api_msgs::msg::Emergency>(
+    "/api/autoware/get/emergency", 10, std::bind(&AutowareStatePanel::onEmergencyStatus, this, _1));
 
   client_engage_ = raw_node_->create_client<tier4_external_api_msgs::srv::Engage>(
-    "/api/autoware/set/engage", rmw_qos_profile_services_default);
+    "/api/external/set/engage", rmw_qos_profile_services_default);
+
+  client_emergency_stop_ = raw_node_->create_client<tier4_external_api_msgs::srv::SetEmergency>(
+    "/api/autoware/set/emergency", rmw_qos_profile_services_default);
 
   pub_velocity_limit_ = raw_node_->create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
-    "/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<tier4_control_msgs::msg::GateMode>(
-    "/control/gate_mode_cmd", rclcpp::QoS(1));
+    "/control/gate_mode_cmd", rclcpp::QoS{1}.transient_local());
 
   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));
+    rclcpp::QoS{1}.transient_local());
 }
 
 void AutowareStatePanel::onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg)
@@ -248,12 +259,25 @@ 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_)));
 }
 
+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<tier4_planning_msgs::msg::VelocityLimit>();
@@ -281,6 +305,29 @@ void AutowareStatePanel::onClickAutowareEngage()
     });
 }
 
+void AutowareStatePanel::onClickEmergencyButton()
+{
+  auto request = std::make_shared<tier4_external_api_msgs::srv::SetEmergency::Request>();
+  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<tier4_external_api_msgs::srv::SetEmergency>::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"

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -24,11 +24,13 @@
 #include <rviz_common/panel.hpp>
 
 #include <autoware_auto_system_msgs/msg/autoware_state.hpp>
-#include <autoware_auto_vehicle_msgs/msg/engage.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <tier4_control_msgs/msg/external_command_selector_mode.hpp>
 #include <tier4_control_msgs/msg/gate_mode.hpp>
+#include <tier4_external_api_msgs/msg/emergency.hpp>
+#include <tier4_external_api_msgs/msg/engage_status.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
+#include <tier4_external_api_msgs/srv/set_emergency.hpp>
 #include <tier4_planning_msgs/msg/approval.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 
@@ -47,14 +49,16 @@ public Q_SLOTS:
   void onClickVelocityLimit();
   void onClickGateMode();
   void onClickPathChangeApproval();
+  void onClickEmergencyButton();
 
 protected:
   void onGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg);
   void onSelectorMode(
     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 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_;
   rclcpp::Subscription<tier4_control_msgs::msg::GateMode>::SharedPtr sub_gate_mode_;
@@ -63,9 +67,11 @@ public Q_SLOTS:
   rclcpp::Subscription<autoware_auto_system_msgs::msg::AutowareState>::SharedPtr
     sub_autoware_state_;
   rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr sub_gear_;
-  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::Engage>::SharedPtr sub_engage_;
+  rclcpp::Subscription<tier4_external_api_msgs::msg::EngageStatus>::SharedPtr sub_engage_;
 
   rclcpp::Client<tier4_external_api_msgs::srv::Engage>::SharedPtr client_engage_;
+  rclcpp::Client<tier4_external_api_msgs::srv::SetEmergency>::SharedPtr client_emergency_stop_;
+  rclcpp::Subscription<tier4_external_api_msgs::msg::Emergency>::SharedPtr sub_emergency_;
 
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
   rclcpp::Publisher<tier4_control_msgs::msg::GateMode>::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

sensing/gnss_poser/CMakeLists.txt

@@ -37,4 +37,5 @@ rclcpp_components_register_node(gnss_poser_node
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

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

sensing/gnss_poser/include/gnss_poser/convert.hpp

@@ -17,6 +17,7 @@
 #include "gnss_poser/gnss_stat.hpp"
 
 #include <GeographicLib/Geoid.hpp>
+#include <GeographicLib/LocalCartesian.hpp>
 #include <GeographicLib/MGRS.hpp>
 #include <GeographicLib/UTMUPS.hpp>
 #include <geo_pos_conv/geo_pos_conv.hpp>
@@ -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)
 {

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

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

sensing/gnss_poser/launch/gnss_poser.launch.xml

@@ -2,6 +2,7 @@
 <launch>
   <arg name="input_topic_fix" default="/fix"/>
   <arg name="input_topic_navpvt" default="/navpvt"/>
+  <arg name="param_file" default="$(find-pkg-share gnss_poser)/config/set_local_origin.param.yaml"/>
 
   <arg name="output_topic_gnss_pose" default="gnss_pose"/>
   <arg name="output_topic_gnss_pose_cov" default="gnss_pose_cov"/>
@@ -12,7 +13,7 @@
   <arg name="gnss_frame" default="gnss"/>
   <arg name="map_frame" default="map"/>
 
-  <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE"/>
+  <arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesian"/>
   <arg name="buff_epoch" default="1"/>
   <arg name="use_ublox_receiver" default="false"/>
   <arg name="plane_zone" default="9"/>
@@ -34,5 +35,6 @@
     <param name="buff_epoch" value="$(var buff_epoch)"/>
     <param name="use_ublox_receiver" value="$(var use_ublox_receiver)"/>
     <param name="plane_zone" value="$(var plane_zone)"/>
+    <param from="$(var param_file)"/>
   </node>
 </launch>

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<int>(CoordinateSystem::MGRS));
   coordinate_system_ = static_cast<CoordinateSystem>(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(),