feat(interface): remove incompatible interface (autowarefoundation#35)

* not use ublox_msg when run as autoware

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* remove twist/kalman/twist & use twist_estimator/twist_with_covariance

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* update particle_array stamp even if the time stamp seems wrong

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

---------

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/gnss_particle_corrector.hpp

@@ -51,14 +51,16 @@ class GnssParticleCorrector : public AbstCorrector
   rclcpp::Subscription<NavPVT>::SharedPtr ublox_sub_;
   rclcpp::Subscription<PoseCovStamped>::SharedPtr pose_sub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr marker_pub_;
-  rclcpp::Publisher<PoseStamped>::SharedPtr direction_pub_;
 
   Float32 latest_height_;
   Eigen::Vector3f last_mean_position_;
 
   void on_ublox(const NavPVT::ConstSharedPtr ublox_msg);
   void on_pose(const PoseCovStamped::ConstSharedPtr pose_msg);
 
+  void process(
+    const Eigen::Vector3f & gnss_position, const rclcpp::Time & stamp, const bool is_rtk_fixed);
+
   bool is_gnss_observation_valid(
     const Eigen::Matrix3f & sigma, const Eigen::Vector3f & meaned_position,
     const Eigen::Vector3f & gnss_position);

localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_core.cpp

@@ -33,27 +33,15 @@ GnssParticleCorrector::GnssParticleCorrector()
   auto on_pose = std::bind(&GnssParticleCorrector::on_pose, this, _1);
   auto on_ublox = std::bind(&GnssParticleCorrector::on_ublox, this, _1);
   auto on_height = [this](const Float32 & height) { this->latest_height_ = height; };
-  ublox_sub_ = create_subscription<NavPVT>("input/navpvt", 10, on_ublox);
-  pose_sub_ = create_subscription<PoseCovStamped>("input/pose_with_covariance", 10, on_pose);
+  if (declare_parameter<bool>("use_ublox_msg", true)) {
+    ublox_sub_ = create_subscription<NavPVT>("input/navpvt", 10, on_ublox);
+  } else {
+    pose_sub_ = create_subscription<PoseCovStamped>("input/pose_with_covariance", 10, on_pose);
+  }
   height_sub_ = create_subscription<Float32>("input/height", 10, on_height);
 
   // Publisher
   marker_pub_ = create_publisher<MarkerArray>("gnss/range_marker", 10);
-  direction_pub_ = create_publisher<PoseStamped>("gnss/direction", 10);
-}
-
-void GnssParticleCorrector::on_pose(const PoseCovStamped::ConstSharedPtr pose_msg)
-{
-  const rclcpp::Time stamp = pose_msg->header.stamp;
-  std::optional<ParticleArray> opt_particles = get_synchronized_particle_array(stamp);
-  if (!opt_particles.has_value()) return;
-
-  auto position = pose_msg->pose.pose.position;
-  Eigen::Vector3f position_vec3f;
-  position_vec3f << position.x, position.y, position.z;
-
-  ParticleArray weighted_particles{weight_particles(opt_particles.value(), position_vec3f, true)};
-  set_weighted_particle_array(weighted_particles);
 }
 
 Eigen::Vector3f extract_enu_vel(const GnssParticleCorrector::NavPVT & msg)
@@ -83,6 +71,17 @@ bool GnssParticleCorrector::is_gnss_observation_valid(
   return true;
 }
 
+void GnssParticleCorrector::on_pose(const PoseCovStamped::ConstSharedPtr pose_msg)
+{
+  const rclcpp::Time stamp = pose_msg->header.stamp;
+  const auto & position = pose_msg->pose.pose.position;
+  Eigen::Vector3f gnss_position;
+  gnss_position << position.x, position.y, position.z;
+
+  constexpr bool is_rtk_fixed = false;
+  process(gnss_position, stamp, is_rtk_fixed);
+}
+
 void GnssParticleCorrector::on_ublox(const NavPVT::ConstSharedPtr ublox_msg)
 {
   const rclcpp::Time stamp = common::ublox_time_to_stamp(*ublox_msg);
@@ -97,29 +96,14 @@ void GnssParticleCorrector::on_ublox(const NavPVT::ConstSharedPtr ublox_msg)
       return;
     }
   }
+  process(gnss_position, stamp, is_rtk_fixed);
+}
 
+void GnssParticleCorrector::process(
+  const Eigen::Vector3f & gnss_position, const rclcpp::Time & stamp, const bool is_rtk_fixed)
+{
   publish_marker(gnss_position, is_rtk_fixed);
 
-  const Eigen::Vector3f doppler = extract_enu_vel(*ublox_msg);
-  const float theta = std::atan2(doppler.y(), doppler.x());
-
-  // visualize doppler velocity
-  {
-    if (doppler.norm() > 1) {
-      PoseStamped pose;
-      pose.header.frame_id = "map";
-      pose.header.stamp = stamp;
-      pose.pose.position.x = gnss_position.x();
-      pose.pose.position.y = gnss_position.y();
-      pose.pose.position.z = 0;
-      pose.pose.orientation.w = std::cos(theta / 2);
-      pose.pose.orientation.z = std::sin(theta / 2);
-      pose.pose.orientation.x = 0;
-      pose.pose.orientation.y = 0;
-      direction_pub_->publish(pose);
-    }
-  }
-
   std::optional<ParticleArray> opt_particles = get_synchronized_particle_array(stamp);
 
   if (!opt_particles.has_value()) return;
@@ -142,6 +126,8 @@ void GnssParticleCorrector::on_ublox(const NavPVT::ConstSharedPtr ublox_msg)
     weight_particles(opt_particles.value(), gnss_position, is_rtk_fixed);
 
   // NOTE: Not sure whether the correction using orientation is effective.
+  // const Eigen::Vector3f doppler = extract_enu_vel(*ublox_msg);
+  // const float theta = std::atan2(doppler.y(), doppler.x());
   // add_weight_by_orientation(weighted_particles, doppler);
 
   // Compute travel distance from last update position

localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/predictor.hpp

@@ -56,7 +56,6 @@ class Predictor : public rclcpp::Node
   // Subscriber
   rclcpp::Subscription<PoseCovStamped>::SharedPtr initialpose_sub_;
   rclcpp::Subscription<TwistCovStamped>::SharedPtr twist_cov_sub_;
-  rclcpp::Subscription<TwistStamped>::SharedPtr twist_sub_;
   rclcpp::Subscription<ParticleArray>::SharedPtr particles_sub_;
   rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr height_sub_;
 
@@ -82,7 +81,7 @@ class Predictor : public rclcpp::Node
 
   // Callback
   void on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose);
-  void on_twist(const TwistStamped::ConstSharedPtr twist);
+  void on_twist_cov(const TwistCovStamped::ConstSharedPtr twist);
   void on_weighted_particles(const ParticleArray::ConstSharedPtr weighted_particles);
   void on_timer();
 

localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/predictor.cpp

@@ -48,18 +48,14 @@ Predictor::Predictor()
   // Subscribers
   using std::placeholders::_1;
   auto on_initial = std::bind(&Predictor::on_initial_pose, this, _1);
-  auto on_twist = std::bind(&Predictor::on_twist, this, _1);
-  // clang-format off
-  auto on_twist_cov= [this](const TwistCovStamped & twist_cov) -> void { this->latest_twist_opt_ = twist_cov; };
-  // clang-format on
+  auto on_twist_cov = std::bind(&Predictor::on_twist_cov, this, _1);
   auto on_particle = std::bind(&Predictor::on_weighted_particles, this, _1);
   auto on_height = [this](std_msgs::msg::Float32 m) -> void { this->ground_height_ = m.data; };
 
   initialpose_sub_ = create_subscription<PoseCovStamped>("initialpose", 1, on_initial);
-  twist_sub_ = create_subscription<TwistStamped>("twist", 10, on_twist);
-  twist_cov_sub_ = create_subscription<TwistCovStamped>("twist_cov", 10, on_twist_cov);
   particles_sub_ = create_subscription<ParticleArray>("weighted_particles", 10, on_particle);
   height_sub_ = create_subscription<std_msgs::msg::Float32>("height", 10, on_height);
+  twist_cov_sub_ = create_subscription<TwistCovStamped>("twist_cov", 10, on_twist_cov);
 
   // Timer callback
   const double prediction_rate = declare_parameter("prediction_rate", 50.0f);
@@ -120,11 +116,12 @@ void Predictor::initialize_particles(const PoseCovStamped & initialpose)
   resampler_ptr_ = std::make_unique<RetroactiveResampler>(number_of_particles_, 100);
 }
 
-void Predictor::on_twist(const TwistStamped::ConstSharedPtr twist)
+void Predictor::on_twist_cov(const TwistCovStamped::ConstSharedPtr twist_cov)
 {
+  const auto twist = twist_cov->twist;
   TwistCovStamped twist_covariance;
-  twist_covariance.header = twist->header;
-  twist_covariance.twist.twist = twist->twist;
+  twist_covariance.header = twist_cov->header;
+  twist_covariance.twist.twist = twist.twist;
   twist_covariance.twist.covariance.at(0) = static_linear_covariance_;
   twist_covariance.twist.covariance.at(7) = 1e4;
   twist_covariance.twist.covariance.at(14) = 1e4;
@@ -196,6 +193,7 @@ void Predictor::on_timer()
   // NOTE: Sometimes particle_array.header.stamp is ancient due to lagged pose_initializer
   if (dt < 0.0 || dt > 1.0) {
     RCLCPP_WARN_STREAM(get_logger(), "time stamp is wrong? " << dt);
+    particle_array_opt_->header.stamp = current_time;
     return;
   }
 

localization/yabloc/twist/twist_estimator/include/twist_estimator/twist_estimator.hpp

@@ -53,11 +53,10 @@ class TwistEstimator : public rclcpp::Node
     SCALE = 3,
   };
 
-  rclcpp::Publisher<TwistStamped>::SharedPtr pub_twist_;
   rclcpp::Publisher<PoseStamped>::SharedPtr pub_pose_;
   rclcpp::Publisher<String>::SharedPtr pub_string_;
   rclcpp::Publisher<Float>::SharedPtr pub_doppler_vel_;
-  rclcpp::Publisher<TwistCovStamped>::SharedPtr pub_twist_with_covariance_;
+  rclcpp::Publisher<TwistCovStamped>::SharedPtr pub_twist_cov_;
 
   rclcpp::Subscription<NavPVT>::SharedPtr sub_navpvt_;
   rclcpp::Subscription<Imu>::SharedPtr sub_imu_;

localization/yabloc/twist/twist_estimator/src/twist_estimator_core.cpp

@@ -41,11 +41,10 @@ TwistEstimator::TwistEstimator()
     create_subscription<VelocityReport>("/vehicle/status/velocity_status", 10, cb_velocity);
   sub_navpvt_ = create_subscription<NavPVT>("/sensing/gnss/ublox/navpvt", 10, cb_pvt);
 
-  pub_twist_ = create_publisher<TwistStamped>("twist", 10);
   pub_pose_ = create_publisher<PoseStamped>("doppler", 10);
   pub_string_ = create_publisher<String>("status", 10);
   pub_doppler_vel_ = create_publisher<Float>("doppler_vel", 10);
-  pub_twist_with_covariance_ = create_publisher<TwistCovStamped>("twist_with_cov", 10);
+  pub_twist_cov_ = create_publisher<TwistCovStamped>("twist_with_cov", 10);
 
   // rotation, velocity, bias, scale
   state_ = Eigen::Vector4f(0, 20, 0, 1);
@@ -129,15 +128,13 @@ void TwistEstimator::publish_twist(const Imu & imu)
     msg.twist.linear.x = static_scale_factor_ * last_wheel_vel_;
   }
 
-  pub_twist_->publish(msg);
-
   {
     TwistCovStamped cov_msg;
     cov_msg.header = msg.header;
     cov_msg.twist.twist = msg.twist;
     cov_msg.twist.covariance.at(0) = 4.0;
     cov_msg.twist.covariance.at(35) = 0.1;
-    pub_twist_with_covariance_->publish(cov_msg);
+    pub_twist_cov_->publish(cov_msg);
   }
 }
 

localization/yabloc/yabloc_launch/launch/impl/pf.launch.xml

@@ -1,10 +1,6 @@
 <launch>
     <arg name="use_sim_time" default="true"/>
 
-    <arg name="resized_image" default="/sensing/camera/undistorted/image_raw"/>
-    <arg name="resized_info" default="/sensing/camera/undistorted/camera_info"/>
-
-    <arg name="predict_twist" default="/localization/twist/kalman/twist"/>
 
     <arg name="static_linear_covariance" default="12.00"/>
     <arg name="static_angular_covariance" default="0.005"/>
@@ -14,12 +10,14 @@
     <arg name="ignore_less_than_float" default="true"/>
     <arg name="gnss_mahalanobis_distance_threshold" default="20.0" description="If the distance to GNSS observation exceeds this, the correction is skipped."/>
 
+    <arg name="use_ublox_msg_in_gnss_corrector" default="true" description="Raw sensor data should not be used when running as Autoware."/>
+    <arg name="twist_cov_for_prediction" default="/localization/twist_estimator/twist_with_covariance"/>
+
     <!-- predict update -->
     <arg name="input_initialpose" default="/localization/initializer/rectified/initialpose"/>
 
     <node pkg="modularized_particle_filter" exec="predictor_node" name="predictor_node" output="screen" args="--ros-args --log-level info">
-        <remap from="twist" to="$(var predict_twist)" />
-
+        <remap from="twist_cov" to="$(var twist_cov_for_prediction)"/>
         <remap from="initialpose" to="$(var input_initialpose)" />
 
         <param name="use_sim_time" value="$(var use_sim_time)"/>
@@ -34,7 +32,6 @@
 
         <remap from="particles_marker_array" to="$(var output_particles_marker_array)"/>
         <remap from="height" to="/localization/map/height"/>
-        <remap from="twist_cov" to="/localization/twist/disabled/twist_with_cov"/>
         <remap from="pose_with_covariance" to="/localization/pose_estimator/pose_with_covariance"/>
 
         <param name="is_swap_mode" value="false" />
@@ -48,8 +45,6 @@
     <arg name="output_scored_cloud" default="scored_cloud"/>
     <arg name="output_cost_map_range" default="cost_map_range"/>
     <node name="camera_corrector" pkg="camera_particle_corrector" exec="camera_particle_corrector_node" output="screen" args="--ros-args --log-level warn">
-        <param name="visualize" value="false"/>
-
         <param name="use_sim_time" value="$(var use_sim_time)"/>
         <param name="image_size" value="800"/>
         <param name="max_range" value="40.0"/>
@@ -73,11 +68,13 @@
         <param name="ignore_less_than_float" value="$(var ignore_less_than_float)"/>
         <param name="for_fixed/max_weight" value="5.0"/>
         <param name="mahalanobis_distance_threshold" value="$(var gnss_mahalanobis_distance_threshold)"/>
+        <param name="use_ublox_msg" value="$(var use_ublox_msg_in_gnss_corrector)"/>
 
         <remap from="input/navpvt" to="/sensing/gnss/ublox/navpvt"/>
         <remap from="input/height" to="/localization/map/height"/>
+        <remap from="input/pose_with_covariance" to="/sensing/gnss/pose_with_covariance"/>
+
         <remap from="weighted_particles" to="$(var inout_weighted_particles)"/>
-        <remap from="pose_with_covariance" to="/localization/pf/disabled/pose_with_covariance"/>
         <remap from="gnss_range_marker" to="/localization/pf/gnss_range_marker"/>
     </node>
 

localization/yabloc/yabloc_launch/launch/impl/twist.launch.xml

@@ -8,7 +8,6 @@
     <group>
         <push-ros-namespace namespace="kalman"/>
 
-        <arg name="output_twist" default="twist"/>
         <arg name="output_doppler" default="doppler"/>
         <arg name="output_status" default="status"/>
         <arg name="output_doppler_vel" default="doppler_vel"/>
@@ -20,7 +19,6 @@
             <param name="static_scale_factor" value="$(var static_scale_factor)"/>
             <param name="static_gyro_bias" value="$(var static_gyro_bias)"/>
 
-            <remap from="twist" to="$(var output_twist)"/>
             <remap from="doppler" to="$(var output_doppler)"/>
             <remap from="status" to="$(var output_status)"/>
             <remap from="doppler_vel" to="$(var output_doppler_vel)"/>