change everything

Signed-off-by: M. Fatih Cırıt <[email protected]>

localization/autoware_pose_covariance_modifier/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_pose_covariance_modifier)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/pose_covariance_modifier.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::pose_covariance_modifier::PoseCovarianceModifierNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)

localization/autoware_pose_covariance_modifier/config/pose_covariance_modifier.param.yaml

@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    # If GNSS yaw standard deviation values are larger than this, trust only NDT
+    threshold_gnss_stddev_yaw_deg_max: 0.3
+
+    # If GNSS position Z standard deviation values are larger than this, trust only NDT
+    threshold_gnss_stddev_z_max: 0.1
+
+    # If GNSS position XY standard deviation values are lower than this, trust only GNSS
+    threshold_gnss_stddev_xy_bound_lower: 0.1
+
+    # If GNSS position XY standard deviation values are higher than this, trust only NDT
+    threshold_gnss_stddev_xy_bound_upper: 0.25
+
+    # If GNSS data is not received for this duration, trust only NDT
+    gnss_pose_timeout_sec: 1.0
+
+    enable_debug_topics: true

localization/autoware_pose_covariance_modifier_node/package.xml → localization/autoware_pose_covariance_modifier/package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>autoware_pose_covariance_modifier_node</name>
+  <name>autoware_pose_covariance_modifier</name>
   <version>1.0.0</version>
-  <description>Converts an autoware_msgs message to autoware_auto_msgs version and publishes it.</description>
+  <description> Add a description. </description>
 
   <maintainer email="[email protected]">Melike Tanrikulu</maintainer>
 
@@ -15,8 +15,10 @@
   <build_depend>rosidl_default_generators</build_depend>
 
   <depend>geometry_msgs</depend>
+  <depend>interpolation</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

localization/autoware_pose_covariance_modifier/schema/pose_covariance_modifier.schema.json

@@ -0,0 +1,67 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Pose Covariance Modifier Node Parameters",
+  "type": "object",
+  "actual_parameters": {
+    "type": "object",
+    "properties": {
+      "threshold_gnss_stddev_yaw_deg_max": {
+        "type": "number",
+        "default": 0.3,
+        "description": "If GNSS yaw standard deviation values are larger than this, trust only NDT"
+      },
+      "threshold_gnss_stddev_z_max": {
+        "type": "number",
+        "default": 0.1,
+        "description": "If GNSS position Z standard deviation values are larger than this, trust only NDT"
+      },
+      "threshold_gnss_stddev_xy_bound_lower": {
+        "type": "number",
+        "default": 0.1,
+        "description": "If GNSS position XY standard deviation values are lower than this, trust only GNSS"
+      },
+      "threshold_gnss_stddev_xy_bound_upper": {
+        "type": "number",
+        "default": 0.25,
+        "description": "If GNSS position XY standard deviation values are higher than this, trust only NDT"
+      },
+      "gnss_pose_timeout_sec": {
+        "type": "number",
+        "default": 1.0,
+        "description": "If GNSS data is not received for this duration, trust only NDT"
+      },
+      "enable_debug_topics": {
+        "type": "boolean",
+        "default": true,
+        "description": "Publish additional debug topics"
+      }
+    },
+    "required": [
+      "threshold_gnss_stddev_yaw_deg_max",
+      "threshold_gnss_stddev_z_max",
+      "threshold_gnss_stddev_xy_bound_lower",
+      "threshold_gnss_stddev_xy_bound_upper",
+      "gnss_pose_timeout_sec",
+      "enable_debug_topics"
+    ],
+    "additionalProperties": false
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/actual_parameters"
+        }
+      },
+      "required": [
+        "ros__parameters"
+      ],
+      "additionalProperties": false
+    }
+  },
+  "required": [
+    "/**"
+  ],
+  "additionalProperties": false
+}

localization/autoware_pose_covariance_modifier_node/src/include/autoware_pose_covariance_modifier_node.hpp → localization/autoware_pose_covariance_modifier/src/include/pose_covariance_modifier.hpp

@@ -17,59 +17,71 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include <std_msgs/msg/string.hpp>
 
 #include <string>
 
-class AutowarePoseCovarianceModifierNode : public rclcpp::Node
+namespace autoware
+{
+namespace pose_covariance_modifier
+{
+class PoseCovarianceModifierNode : public rclcpp::Node
 {
 public:
-  AutowarePoseCovarianceModifierNode();
-
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
-    gnss_pose_with_cov_sub_;
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
-    ndt_pose_with_cov_sub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
-    output_pose_with_covariance_stamped_pub_;
-  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pose_source_pub_;
-  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr out_ndt_position_stddev_pub_;
-  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr out_gnss_position_stddev_pub_;
-
-  void gnss_pose_with_cov_callback(
-    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
-  void ndt_pose_with_cov_callback(
-    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
-  std::array<double, 36> ndt_covariance_modifier(std::array<double, 36> & ndt_covariance_in);
-  geometry_msgs::msg::PoseWithCovarianceStamped gnss_source_pose_with_cov;
-
-  void check_gnss_pose_timeout();
-
-private:
-  void update_pose_source_based_on_stddev(
-    double gnss_pose_average_stddev_xy, double gnss_pose_stddev_z,
-    double gnss_pose_yaw_stddev_in_degrees);
+  PoseCovarianceModifierNode(const rclcpp::NodeOptions & node_options);
 
   enum class PoseSource {
     GNSS = 0,
     GNSS_NDT = 1,
     NDT = 2,
   };
 
-  rclcpp::Time gnss_pose_last_received_time_;
+private:
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    sub_gnss_pose_with_cov_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    sub_ndt_pose_with_cov_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    pub_pose_with_covariance_stamped_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_str_pose_source_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_double_ndt_position_stddev_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_double_gnss_position_stddev_;
 
-  PoseSource pose_source_;
+  rclcpp::Time gnss_pose_received_time_last_;
+  geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr gnss_pose_with_cov_last_;
+
+  double threshold_gnss_stddev_yaw_deg_max_;
+  double threshold_gnss_stddev_z_max_;
+  double threshold_gnss_stddev_xy_bound_lower_;
+  double threshold_gnss_stddev_xy_bound_upper_;
 
-  double gnss_stddev_reliable_max_, gnss_stddev_unreliable_min_, yaw_stddev_deg_threshold_;
   double gnss_pose_timeout_sec_;
-  bool debug_;
+  bool debug_mode_;
+  PoseSource pose_source_;
 
   // covariance matrix indexes
   const int X_POS_IDX_ = 0;
   const int Y_POS_IDX_ = 7;
   const int Z_POS_IDX_ = 14;
   const int YAW_POS_IDX_ = 35;
+
+  void callback_gnss_pose_with_cov(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
+  void callback_ndt_pose_with_cov(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg_pose_with_cov_in);
+  std::array<double, 36> update_ndt_covariances_from_gnss(
+    const std::array<double, 36> & ndt_covariance_in);
+
+  bool gnss_pose_has_timed_out(const rclcpp::Time & gnss_pose_received_time_last);
+
+  PoseSource pose_source_from_gnss_stddev(
+    double gnss_pose_yaw_stddev_deg, double gnss_pose_stddev_z, double gnss_pose_stddev_xy) const;
+
+  void publish_pose_type(const PoseSource & pose_source);
 };
 
+}  // namespace pose_covariance_modifier
+}  // namespace autoware
+
 #endif  // AUTOWARE_POSE_COVARIANCE_MODIFIER_NODE_HPP_