fix conflict

Signed-off-by: scepter914 <[email protected]>

common/object_recognition_utils/include/object_recognition_utils/transform.hpp

@@ -15,15 +15,22 @@
 #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
 #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
 
+#include <pcl_ros/transforms.hpp>
+
 #include <geometry_msgs/msg/transform.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/point_field.hpp>
 
 #include <boost/optional.hpp>
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
+#include <tf2_eigen/tf2_eigen.hpp>
+
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
@@ -45,6 +52,23 @@ namespace detail
     return boost::none;
   }
 }
+
+[[maybe_unused]] inline boost::optional<Eigen::Matrix4f> getTransformMatrix(
+  const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header,
+  const tf2_ros::Buffer & tf_buffer)
+{
+  try {
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped = tf_buffer.lookupTransform(
+      in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp,
+      rclcpp::Duration::from_seconds(1.0));
+    Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
+    return mat;
+  } catch (tf2::TransformException & e) {
+    RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what());
+    return boost::none;
+  }
+}
 }  // namespace detail
 
 namespace object_recognition_utils
@@ -79,6 +103,46 @@ bool transformObjects(
   }
   return true;
 }
+template <class T>
+bool transformObjectsWithFeature(
+  const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
+  T & output_msg)
+{
+  output_msg = input_msg;
+  if (input_msg.header.frame_id != target_frame_id) {
+    output_msg.header.frame_id = target_frame_id;
+    tf2::Transform tf_target2objects_world;
+    tf2::Transform tf_target2objects;
+    tf2::Transform tf_objects_world2objects;
+    const auto ros_target2objects_world = detail::getTransform(
+      tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
+    if (!ros_target2objects_world) {
+      return false;
+    }
+    const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer);
+    if (!tf_matrix) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix");
+      return false;
+    }
+    for (auto & feature_object : output_msg.feature_objects) {
+      // transform object
+      tf2::fromMsg(
+        feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
+      tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
+      tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose);
+
+      // transform cluster
+      sensor_msgs::msg::PointCloud2 transformed_cluster;
+      pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster);
+      transformed_cluster.header.frame_id = target_frame_id;
+      feature_object.feature.cluster = transformed_cluster;
+    }
+    output_msg.header.frame_id = target_frame_id;
+    return true;
+  }
+  return true;
+}
 }  // namespace object_recognition_utils
 
 #endif  // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_

common/object_recognition_utils/package.xml

@@ -17,7 +17,13 @@
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>libboost-dev</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
   <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp

@@ -447,7 +447,8 @@ class MPC
    * @param param Trajectory filtering parameters.
    */
   void setReferenceTrajectory(
-    const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param);
+    const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
+    const Odometry & current_kinematics);
 
   /**
    * @brief Reset the previous result of MPC.

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -181,7 +181,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
    * @brief Set the current trajectory using the received message.
    * @param msg Received trajectory message.
    */
-  void setTrajectory(const Trajectory & msg);
+  void setTrajectory(const Trajectory & msg, const Odometry & current_kinematics);
 
   /**
    * @brief Check if the received data is valid.

control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp

@@ -99,7 +99,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<do
  * @return The pair contains the successful flag and the resultant resampled trajectory
  */
 std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
-  const MPCTrajectory & input, const double resample_interval_dist);
+  const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx,
+  const double ego_offset_to_segment);
 
 /**
  * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired
@@ -122,14 +123,14 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj);
 
 /**
  * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing
- * @param [in] start_idx index of the trajectory point from which to start smoothing
- * @param [in] start_vel initial velocity to set at the start_idx
+ * @param [in] start_seg_idx segment index of the trajectory point from which to start smoothing
+ * @param [in] start_vel initial velocity to set at the start_seg_idx
  * @param [in] acc_lim limit on the acceleration
  * @param [in] tau constant to control the smoothing (high-value = very smooth)
  * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity
  */
 void dynamicSmoothingVelocity(
-  const size_t start_idx, const double start_vel, const double acc_lim, const double tau,
+  const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
   MPCTrajectory & traj);
 
 /**

control/mpc_lateral_controller/src/mpc.cpp

@@ -173,13 +173,20 @@ Float32MultiArrayStamped MPC::generateDiagData(
 }
 
 void MPC::setReferenceTrajectory(
-  const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param)
+  const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
+  const Odometry & current_kinematics)
 {
+  const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+    trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
+    ego_nearest_yaw_threshold);
+  const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment(
+    trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);
+
   const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
 
   // resampling
-  const auto [success_resample, mpc_traj_resampled] =
-    MPCUtils::resampleMPCTrajectoryByDistance(mpc_traj_raw, param.traj_resample_dist);
+  const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
+    mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
   if (!success_resample) {
     warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
     return;
@@ -389,13 +396,13 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter(
     return input;
   }
 
-  const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+  const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
     autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
     ego_nearest_yaw_threshold);
 
   MPCTrajectory output = input;
   MPCUtils::dynamicSmoothingVelocity(
-    nearest_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit,
+    nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit,
     m_param.velocity_time_constant, output);
 
   auto last_point = output.back();

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -227,7 +227,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   trajectory_follower::InputData const & input_data)
 {
   // set input data
-  setTrajectory(input_data.current_trajectory);
+  setTrajectory(input_data.current_trajectory, input_data.current_odometry);
 
   m_current_kinematic_state = input_data.current_odometry;
   m_current_steering = input_data.current_steering;
@@ -319,7 +319,7 @@ bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd)
 
 bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data)
 {
-  setTrajectory(input_data.current_trajectory);
+  setTrajectory(input_data.current_trajectory, input_data.current_odometry);
   m_current_kinematic_state = input_data.current_odometry;
   m_current_steering = input_data.current_steering;
 
@@ -339,7 +339,8 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_
   return true;
 }
 
-void MpcLateralController::setTrajectory(const Trajectory & msg)
+void MpcLateralController::setTrajectory(
+  const Trajectory & msg, const Odometry & current_kinematics)
 {
   m_current_trajectory = msg;
 
@@ -353,7 +354,7 @@ void MpcLateralController::setTrajectory(const Trajectory & msg)
     return;
   }
 
-  m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param);
+  m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics);
 
   // update trajectory buffer to check the trajectory shape change.
   m_trajectory_buffer.push_back(m_current_trajectory);

control/mpc_lateral_controller/src/mpc_utils.cpp

@@ -27,6 +27,19 @@
 
 namespace autoware::motion::control::mpc_lateral_controller
 {
+namespace
+{
+double calcLongitudinalOffset(
+  const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back,
+  const geometry_msgs::msg::Point & p_target)
+{
+  const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0};
+  const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0};
+
+  return segment_vec.dot(target_vec) / segment_vec.norm();
+}
+}  // namespace
+
 namespace MPCUtils
 {
 using tier4_autoware_utils::calcDistance2d;
@@ -77,7 +90,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<do
 }
 
 std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
-  const MPCTrajectory & input, const double resample_interval_dist)
+  const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx,
+  const double ego_offset_to_segment)
 {
   MPCTrajectory output;
 
@@ -92,7 +106,18 @@ std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
   }
 
   std::vector<double> output_arclength;
-  for (double s = 0; s < input_arclength.back(); s += resample_interval_dist) {
+  // To accurately sample the ego point, resample separately in the forward direction and the
+  // backward direction from the current position.
+  for (double s = std::clamp(
+         input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0,
+         input_arclength.back() - 1e-6);
+       0 <= s; s -= resample_interval_dist) {
+    output_arclength.push_back(s);
+  }
+  std::reverse(output_arclength.begin(), output_arclength.end());
+  for (double s = std::max(input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0) +
+                  resample_interval_dist;
+       s < input_arclength.back(); s += resample_interval_dist) {
     output_arclength.push_back(s);
   }
 
@@ -274,13 +299,17 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj)
 }
 
 void dynamicSmoothingVelocity(
-  const size_t start_idx, const double start_vel, const double acc_lim, const double tau,
+  const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
   MPCTrajectory & traj)
 {
   double curr_v = start_vel;
-  traj.vx.at(start_idx) = start_vel;
+  // set current velocity in both start and end point of the segment
+  traj.vx.at(start_seg_idx) = start_vel;
+  if (1 < traj.vx.size()) {
+    traj.vx.at(start_seg_idx + 1) = start_vel;
+  }
 
-  for (size_t i = start_idx + 1; i < traj.size(); ++i) {
+  for (size_t i = start_seg_idx + 2; i < traj.size(); ++i) {
     const double ds = calcDistance2d(traj, i, i - 1);
     const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits<double>::epsilon());
     const double a = tau / std::max(tau + dt, std::numeric_limits<double>::epsilon());
@@ -320,29 +349,40 @@ bool calcNearestPoseInterp(
     return true;
   }
 
-  auto calcSquaredDist = [](const Pose & p, const MPCTrajectory & t, const size_t idx) {
-    const double dx = p.position.x - t.x.at(idx);
-    const double dy = p.position.y - t.y.at(idx);
-    return dx * dx + dy * dy;
-  };
-
   /* get second nearest index = next to nearest_index */
-  const size_t next = static_cast<size_t>(
-    std::min(static_cast<int>(*nearest_index) + 1, static_cast<int>(traj_size) - 1));
-  const size_t prev =
-    static_cast<size_t>(std::max(static_cast<int>(*nearest_index) - 1, static_cast<int>(0)));
-  const double dist_to_next = calcSquaredDist(self_pose, traj, next);
-  const double dist_to_prev = calcSquaredDist(self_pose, traj, prev);
-  const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev;
-
-  const double a_sq = calcSquaredDist(self_pose, traj, *nearest_index);
-  const double b_sq = calcSquaredDist(self_pose, traj, second_nearest_index);
-  const double dx3 = traj.x.at(*nearest_index) - traj.x.at(second_nearest_index);
-  const double dy3 = traj.y.at(*nearest_index) - traj.y.at(second_nearest_index);
-  const double c_sq = dx3 * dx3 + dy3 * dy3;
+  const auto [prev, next] = [&]() -> std::pair<size_t, size_t> {
+    if (*nearest_index == 0) {
+      return std::make_pair(0, 1);
+    }
+    if (*nearest_index == traj_size - 1) {
+      return std::make_pair(traj_size - 2, traj_size - 1);
+    }
 
+    geometry_msgs::msg::Point nearest_traj_point;
+    nearest_traj_point.x = traj.x.at(*nearest_index);
+    nearest_traj_point.y = traj.y.at(*nearest_index);
+    geometry_msgs::msg::Point next_nearest_traj_point;
+    next_nearest_traj_point.x = traj.x.at(*nearest_index + 1);
+    next_nearest_traj_point.y = traj.y.at(*nearest_index + 1);
+
+    const double signed_length =
+      calcLongitudinalOffset(nearest_traj_point, next_nearest_traj_point, self_pose.position);
+    if (signed_length <= 0) {
+      return std::make_pair(*nearest_index - 1, *nearest_index);
+    }
+    return std::make_pair(*nearest_index, *nearest_index + 1);
+  }();
+
+  geometry_msgs::msg::Point next_traj_point;
+  next_traj_point.x = traj.x.at(next);
+  next_traj_point.y = traj.y.at(next);
+  geometry_msgs::msg::Point prev_traj_point;
+  prev_traj_point.x = traj.x.at(prev);
+  prev_traj_point.y = traj.y.at(prev);
+  const double traj_seg_length =
+    tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point);
   /* if distance between two points are too close */
-  if (c_sq < 1.0E-5) {
+  if (traj_seg_length < 1.0E-5) {
     nearest_pose->position.x = traj.x.at(*nearest_index);
     nearest_pose->position.y = traj.y.at(*nearest_index);
     nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index));
@@ -351,18 +391,15 @@ bool calcNearestPoseInterp(
   }
 
   /* linear interpolation */
-  const double alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0);
-  nearest_pose->position.x =
-    alpha * traj.x.at(*nearest_index) + (1 - alpha) * traj.x.at(second_nearest_index);
-  nearest_pose->position.y =
-    alpha * traj.y.at(*nearest_index) + (1 - alpha) * traj.y.at(second_nearest_index);
-  const double tmp_yaw_err =
-    normalizeRadian(traj.yaw.at(*nearest_index) - traj.yaw.at(second_nearest_index));
-  const double nearest_yaw =
-    normalizeRadian(traj.yaw.at(second_nearest_index) + alpha * tmp_yaw_err);
+  const double ratio = std::clamp(
+    calcLongitudinalOffset(prev_traj_point, next_traj_point, self_pose.position) / traj_seg_length,
+    0.0, 1.0);
+  nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next);
+  nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next);
+  const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next));
+  const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err);
   nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw);
-  *nearest_time = alpha * traj.relative_time.at(*nearest_index) +
-                  (1 - alpha) * traj.relative_time.at(second_nearest_index);
+  *nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next);
   return true;
 }