Merge branch 'autowarefoundation:tier4/proposal' into 234-route-handl…

…er-for-behavior-path-planner-drivable-area-extension

launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml

@@ -32,3 +32,14 @@
 
     # Number of threads used for parallel computing
     omp_num_threads: 4
+
+    # The covariance of output pose
+    output_pose_covariance:
+      [
+        0.025, 0.0,   0.0,   0.0,      0.0,      0.0,
+        0.0,   0.025, 0.0,   0.0,      0.0,      0.0,
+        0.0,   0.0,   0.025, 0.0,      0.0,      0.0,
+        0.0,   0.0,   0.0,   0.000625, 0.0,      0.0,
+        0.0,   0.0,   0.0,   0.0,      0.000625, 0.0,
+        0.0,   0.0,   0.0,   0.0,      0.0,      0.000625,
+      ]

launch/tier4_localization_launch/launch/util/util.launch.py

@@ -55,7 +55,7 @@ def load_composable_node_param(param_path):
     )
     random_downsample_component = ComposableNode(
         package="pointcloud_preprocessor",
-        plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+        plugin="pointcloud_preprocessor::RandomDownsampleFilterComponent",
         name="random_downsample_filter",
         remappings=[
             ("input", LaunchConfiguration("output_voxel_grid_downsample_sensor_points_topic")),

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -24,7 +24,7 @@
     # The number of iterations required to calculate alignment
     max_iterations: 30
 
-    # Threshold for deciding whetherto trust the estimation result
+    # Threshold for deciding whether to trust the estimation result
     converged_param_transform_probability: 3.0
 
     # neighborhood search method in OMP
@@ -33,3 +33,14 @@
 
     # Number of threads used for parallel computing
     omp_num_threads: 4
+
+    # The covariance of output pose
+    output_pose_covariance:
+      [
+        0.025, 0.0,   0.0,   0.0,      0.0,      0.0,
+        0.0,   0.025, 0.0,   0.0,      0.0,      0.0,
+        0.0,   0.0,   0.025, 0.0,      0.0,      0.0,
+        0.0,   0.0,   0.0,   0.000625, 0.0,      0.0,
+        0.0,   0.0,   0.0,   0.0,      0.000625, 0.0,
+        0.0,   0.0,   0.0,   0.0,      0.0,      0.000625,
+      ]

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -155,6 +155,7 @@ class NDTScanMatcher : public rclcpp::Node
   double converged_param_transform_probability_;
   float inversion_vector_threshold_;
   float oscillation_threshold_;
+  std::array<double, 36> output_pose_covariance_;
 
   std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>
     initial_pose_msg_ptr_array_;

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -165,6 +165,12 @@ NDTScanMatcher::NDTScanMatcher()
   converged_param_transform_probability_ = this->declare_parameter(
     "converged_param_transform_probability", converged_param_transform_probability_);
 
+  std::vector<double> output_pose_covariance =
+    this->declare_parameter<std::vector<double>>("output_pose_covariance");
+  for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
+    output_pose_covariance_[i] = output_pose_covariance[i];
+  }
+
   rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group;
   initial_pose_callback_group =
     this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -510,15 +516,7 @@ void NDTScanMatcher::callbackSensorPoints(
   result_pose_with_cov_msg.header.stamp = sensor_ros_time;
   result_pose_with_cov_msg.header.frame_id = map_frame_;
   result_pose_with_cov_msg.pose.pose = result_pose_msg;
-
-  // TODO(Tier IV): temporary value
-  Eigen::Map<RowMatrixXd> covariance(&result_pose_with_cov_msg.pose.covariance[0], 6, 6);
-  covariance(0, 0) = 0.025;
-  covariance(1, 1) = 0.025;
-  covariance(2, 2) = 0.025;
-  covariance(3, 3) = 0.000625;
-  covariance(4, 4) = 0.000625;
-  covariance(5, 5) = 0.000625;
+  result_pose_with_cov_msg.pose.covariance = output_pose_covariance_;
 
   if (is_converged) {
     ndt_pose_pub_->publish(result_pose_stamped_msg);

localization/ndt_scan_matcher/src/util_func.cpp

@@ -220,12 +220,12 @@ std::vector<geometry_msgs::msg::Pose> createRandomPoseArray(
   const Eigen::Map<const RowMatrixXd> covariance =
     makeEigenCovariance(base_pose_with_cov.pose.covariance);
 
-  std::normal_distribution<> x_distribution(0.0, covariance(0, 0));
-  std::normal_distribution<> y_distribution(0.0, covariance(1, 1));
-  std::normal_distribution<> z_distribution(0.0, covariance(2, 2));
-  std::normal_distribution<> roll_distribution(0.0, covariance(3, 3));
-  std::normal_distribution<> pitch_distribution(0.0, covariance(4, 4));
-  std::normal_distribution<> yaw_distribution(0.0, covariance(5, 5));
+  std::normal_distribution<> x_distribution(0.0, std::sqrt(covariance(0, 0)));
+  std::normal_distribution<> y_distribution(0.0, std::sqrt(covariance(1, 1)));
+  std::normal_distribution<> z_distribution(0.0, std::sqrt(covariance(2, 2)));
+  std::normal_distribution<> roll_distribution(0.0, std::sqrt(covariance(3, 3)));
+  std::normal_distribution<> pitch_distribution(0.0, std::sqrt(covariance(4, 4)));
+  std::normal_distribution<> yaw_distribution(0.0, std::sqrt(covariance(5, 5)));
 
   const auto base_rpy = getRPY(base_pose_with_cov);
 

localization/pose_initializer/CMakeLists.txt

@@ -29,4 +29,5 @@ endif()
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

localization/pose_initializer/config/pose_initializer.param.yaml

@@ -0,0 +1,46 @@
+/**:
+  ros__parameters:
+
+    # from initialpose (Rviz's 2DPoseEstimate)
+    initialpose_particle_covariance:
+      [
+        4.0, 0.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 4.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 0.0, 0.01, 0.0,  0.0,  0.0,
+        0.0, 0.0, 0.0,  0.01, 0.0,  0.0,
+        0.0, 0.0, 0.0,  0.0,  0.01, 0.0,
+        0.0, 0.0, 0.0,  0.0,  0.0,  1.0,
+      ]
+
+    # from gnss
+    gnss_particle_covariance:
+      [
+        1.0, 0.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 1.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 0.0, 0.01, 0.0,  0.0,  0.0,
+        0.0, 0.0, 0.0,  0.01, 0.0,  0.0,
+        0.0, 0.0, 0.0,  0.0,  0.01, 0.0,
+        0.0, 0.0, 0.0,  0.0,  0.0,  10.0,
+      ]
+
+    # from service
+    service_particle_covariance:
+      [
+        1.0, 0.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 1.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 0.0, 0.01, 0.0,  0.0,  0.0,
+        0.0, 0.0, 0.0,  0.01, 0.0,  0.0,
+        0.0, 0.0, 0.0,  0.0,  0.01, 0.0,
+        0.0, 0.0, 0.0,  0.0,  0.0,  10.0,
+      ]
+
+    # output
+    output_pose_covariance:
+      [
+        1.0, 0.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 1.0, 0.0,  0.0,  0.0,  0.0,
+        0.0, 0.0, 0.01, 0.0,  0.0,  0.0,
+        0.0, 0.0, 0.0,  0.01, 0.0,  0.0,
+        0.0, 0.0, 0.0,  0.0,  0.01, 0.0,
+        0.0, 0.0, 0.0,  0.0,  0.0,  0.2,
+      ]

localization/pose_initializer/include/pose_initializer/pose_initializer_core.hpp

@@ -36,16 +36,15 @@ class PoseInitializer : public rclcpp::Node
 {
 public:
   PoseInitializer();
-  ~PoseInitializer();
 
 private:
-  void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud2_msg_ptr);
+  void callbackMapPoints(sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr);
   void serviceInitializePose(
-    const std::shared_ptr<tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request> req,
-    std::shared_ptr<tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response> res);
+    const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
+    tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res);
   void serviceInitializePoseAuto(
-    const std::shared_ptr<tier4_external_api_msgs::srv::InitializePoseAuto::Request> req,
-    std::shared_ptr<tier4_external_api_msgs::srv::InitializePoseAuto::Response> res);
+    const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr req,
+    tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr res);
   void callbackInitialPose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_cov_msg_ptr);
   void callbackGNSSPoseCov(
@@ -91,6 +90,10 @@ class PoseInitializer : public rclcpp::Node
   uint32_t response_id_ = 0;
 
   bool enable_gnss_callback_;
+  std::array<double, 36> initialpose_particle_covariance_;
+  std::array<double, 36> gnss_particle_covariance_;
+  std::array<double, 36> service_particle_covariance_;
+  std::array<double, 36> output_pose_covariance_;
 };
 
 #endif  // POSE_INITIALIZER__POSE_INITIALIZER_CORE_HPP_

localization/pose_initializer/launch/pose_initializer.launch.xml

@@ -1,4 +1,7 @@
 <launch>
+
+  <arg name="config_file" default="$(find-pkg-share pose_initializer)/config/pose_initializer.param.yaml"/>
+
   <node pkg="pose_initializer" exec="pose_initializer" name="pose_initializer" output="log">
     <remap from="initialpose" to="/initialpose" />
     <remap from="initialpose3d" to="/initialpose3d" />
@@ -7,6 +10,8 @@
     <remap from="ndt_align_srv" to="/localization/pose_estimator/ndt_align_srv" />
     <remap from="service/initialize_pose" to="/localization/util/initialize_pose" />
     <remap from="service/initialize_pose_auto" to="/localization/util/initialize_pose_auto" />
+
+    <param from="$(var config_file)"/>
     <param name="enable_gnss_callback" value="true" />
   </node>
 </launch>

localization/pose_initializer/src/pose_initializer_core.cpp

@@ -46,6 +46,30 @@ PoseInitializer::PoseInitializer()
 {
   enable_gnss_callback_ = this->declare_parameter("enable_gnss_callback", true);
 
+  std::vector<double> initialpose_particle_covariance =
+    this->declare_parameter<std::vector<double>>("initialpose_particle_covariance");
+  for (std::size_t i = 0; i < initialpose_particle_covariance.size(); ++i) {
+    initialpose_particle_covariance_[i] = initialpose_particle_covariance[i];
+  }
+
+  std::vector<double> gnss_particle_covariance =
+    this->declare_parameter<std::vector<double>>("gnss_particle_covariance");
+  for (std::size_t i = 0; i < gnss_particle_covariance.size(); ++i) {
+    gnss_particle_covariance_[i] = gnss_particle_covariance[i];
+  }
+
+  std::vector<double> service_particle_covariance =
+    this->declare_parameter<std::vector<double>>("service_particle_covariance");
+  for (std::size_t i = 0; i < service_particle_covariance.size(); ++i) {
+    service_particle_covariance_[i] = service_particle_covariance[i];
+  }
+
+  std::vector<double> output_pose_covariance =
+    this->declare_parameter<std::vector<double>>("output_pose_covariance");
+  for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
+    output_pose_covariance_[i] = output_pose_covariance[i];
+  }
+
   // We can't use _1 because pcl leaks an alias to boost::placeholders::_1, so it would be ambiguous
   initial_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "initialpose", 10,
@@ -83,8 +107,6 @@ PoseInitializer::PoseInitializer()
                                         std::placeholders::_1, std::placeholders::_2));
 }
 
-PoseInitializer::~PoseInitializer() {}
-
 void PoseInitializer::callbackMapPoints(
   sensor_msgs::msg::PointCloud2::ConstSharedPtr map_points_msg_ptr)
 {
@@ -94,21 +116,15 @@ void PoseInitializer::callbackMapPoints(
 }
 
 void PoseInitializer::serviceInitializePose(
-  const std::shared_ptr<tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request> req,
-  std::shared_ptr<tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response> res)
+  const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req,
+  tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res)
 {
   enable_gnss_callback_ = false;  // get only first topic
 
   auto add_height_pose_msg_ptr = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
   getHeight(req->pose_with_covariance, add_height_pose_msg_ptr);
 
-  // TODO(YamatoAndo)
-  add_height_pose_msg_ptr->pose.covariance[0] = 1.0;
-  add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 1.0;
-  add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 1.0;
+  add_height_pose_msg_ptr->pose.covariance = service_particle_covariance_;
 
   res->success = callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
 }
@@ -121,13 +137,7 @@ void PoseInitializer::callbackInitialPose(
   auto add_height_pose_msg_ptr = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
   getHeight(*pose_cov_msg_ptr, add_height_pose_msg_ptr);
 
-  // TODO(YamatoAndo)
-  add_height_pose_msg_ptr->pose.covariance[0] = 2.0;
-  add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 2.0;
-  add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 0.3;
+  add_height_pose_msg_ptr->pose.covariance = initialpose_particle_covariance_;
 
   callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
 }
@@ -145,21 +155,17 @@ void PoseInitializer::callbackGNSSPoseCov(
   auto add_height_pose_msg_ptr = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
   getHeight(*pose_cov_msg_ptr, add_height_pose_msg_ptr);
 
-  // TODO(YamatoAndo)
-  add_height_pose_msg_ptr->pose.covariance[0] = 1.0;
-  add_height_pose_msg_ptr->pose.covariance[1 * 6 + 1] = 1.0;
-  add_height_pose_msg_ptr->pose.covariance[2 * 6 + 2] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[3 * 6 + 3] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[4 * 6 + 4] = 0.01;
-  add_height_pose_msg_ptr->pose.covariance[5 * 6 + 5] = 3.14;
+  add_height_pose_msg_ptr->pose.covariance = gnss_particle_covariance_;
 
   callAlignServiceAndPublishResult(add_height_pose_msg_ptr);
 }
 
 void PoseInitializer::serviceInitializePoseAuto(
-  const std::shared_ptr<tier4_external_api_msgs::srv::InitializePoseAuto::Request> req,
-  std::shared_ptr<tier4_external_api_msgs::srv::InitializePoseAuto::Response> res)
+  const tier4_external_api_msgs::srv::InitializePoseAuto::Request::SharedPtr req,
+  tier4_external_api_msgs::srv::InitializePoseAuto::Response::SharedPtr res)
 {
+  (void)req;
+
   RCLCPP_INFO(this->get_logger(), "Called Pose Initialize Service");
   enable_gnss_callback_ = true;
   res->status = tier4_api_utils::response_success();
@@ -226,12 +232,7 @@ bool PoseInitializer::callAlignServiceAndPublishResult(
         // NOTE temporary cov
         geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_covariance =
           result.get()->pose_with_covariance;
-        pose_with_covariance.pose.covariance[0] = 1.0;
-        pose_with_covariance.pose.covariance[1 * 6 + 1] = 1.0;
-        pose_with_covariance.pose.covariance[2 * 6 + 2] = 0.01;
-        pose_with_covariance.pose.covariance[3 * 6 + 3] = 0.01;
-        pose_with_covariance.pose.covariance[4 * 6 + 4] = 0.01;
-        pose_with_covariance.pose.covariance[5 * 6 + 5] = 0.2;
+        pose_with_covariance.pose.covariance = output_pose_covariance_;
         initial_pose_pub_->publish(pose_with_covariance);
         enable_gnss_callback_ = false;
       } else {

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -44,7 +44,7 @@ static geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection(
   double yaw, pitch, roll;
   tf2::getEulerYPR(obj_pose.orientation, yaw, pitch, roll);
   tf2::Quaternion inv_q;
-  inv_q.setRPY(roll, pitch, -yaw);
+  inv_q.setRPY(roll, pitch, yaw + M_PI);
   obj_pose.orientation = tf2::toMsg(inv_q);
   return obj_pose;
 }