Merge pull request autowarefoundation#171 from tier4/sync-upstream

chore: sync upstream

control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp

@@ -19,8 +19,7 @@
 #include "eigen3/Eigen/Core"
 #include "eigen3/Eigen/Geometry"
 #include "geometry/common_2d.hpp"
-#include "motion_common/motion_common.hpp"
-#include "motion_common/trajectory_common.hpp"
+#include "interpolation/linear_interpolation.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "tf2/utils.h"
 #include "trajectory_follower/visibility_control.hpp"
@@ -50,8 +49,6 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Quaternion;
-namespace motion_common = ::motion::motion_common;
-namespace trajectory_common = ::autoware::motion::motion_common;
 
 /**
  * @brief check if trajectory is invalid or not
@@ -117,27 +114,26 @@ TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint(
 
   const float64_t len_to_interpolated =
     motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position);
-  const float64_t len_segment =
-    trajectory_common::calcSignedArcLength(points, seg_idx, seg_idx + 1);
+  const float64_t len_segment = motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1);
   const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0);
 
   {
     const size_t i = seg_idx;
 
-    interpolated_point.pose.position.x = motion_common::interpolate(
+    interpolated_point.pose.position.x = interpolation::lerp(
       points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
-    interpolated_point.pose.position.y = motion_common::interpolate(
+    interpolated_point.pose.position.y = interpolation::lerp(
       points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
     interpolated_point.pose.orientation = lerpOrientation(
       points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
-    interpolated_point.longitudinal_velocity_mps = motion_common::interpolate(
+    interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
       points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps,
       interpolate_ratio);
-    interpolated_point.lateral_velocity_mps = motion_common::interpolate(
+    interpolated_point.lateral_velocity_mps = interpolation::lerp(
       points.at(i).lateral_velocity_mps, points.at(i + 1).lateral_velocity_mps, interpolate_ratio);
-    interpolated_point.acceleration_mps2 = motion_common::interpolate(
+    interpolated_point.acceleration_mps2 = interpolation::lerp(
       points.at(i).acceleration_mps2, points.at(i + 1).acceleration_mps2, interpolate_ratio);
-    interpolated_point.heading_rate_rps = motion_common::interpolate(
+    interpolated_point.heading_rate_rps = interpolation::lerp(
       points.at(i).heading_rate_rps, points.at(i + 1).heading_rate_rps, interpolate_ratio);
   }
 

control/trajectory_follower/include/trajectory_follower/mpc.hpp

@@ -18,7 +18,6 @@
 #include "common/types.hpp"
 #include "geometry/common_2d.hpp"
 #include "helper_functions/angle_utils.hpp"
-#include "motion_common/motion_common.hpp"
 #include "osqp_interface/osqp_interface.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
@@ -37,9 +36,9 @@
 
 #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
 #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
 #include "geometry_msgs/msg/pose.hpp"
+#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
 
 #include <deque>
 #include <memory>
@@ -395,7 +394,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
     const float64_t current_velocity, const geometry_msgs::msg::Pose & current_pose,
     autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd,
     autoware_auto_planning_msgs::msg::Trajectory & predicted_traj,
-    autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic);
+    tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic);
   /**
    * @brief set the reference trajectory to follow
    */

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -37,13 +37,13 @@
 
 #include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
 #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
 #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
+#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
 
 #include <deque>
 #include <memory>
@@ -80,9 +80,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
 
   //!< @brief topic publisher for predicted trajectory
   rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr m_pub_predicted_traj;
-  //!< @brief topic publisher for control diagnostic
-  rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr
-    m_pub_diagnostic;
+  //!< @brief topic publisher for control debug values
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug_values;
   //!< @brief subscription for transform messages
   rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_sub;
   rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_static_sub;
@@ -175,8 +174,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
    * @brief publish diagnostic message
    * @param [in] diagnostic published diagnostic
    */
-  void publishDiagnostic(
-    autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const;
+  void publishDebugValues(tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic) const;
 
   /**
    * @brief get stop command

control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp

@@ -21,7 +21,6 @@
 #include "helper_functions/angle_utils.hpp"
 #include "interpolation/linear_interpolation.hpp"
 #include "interpolation/spline_interpolation.hpp"
-#include "motion_common/motion_common.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/utils.h"
 

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -18,8 +18,6 @@
 #include "diagnostic_updater/diagnostic_updater.hpp"
 #include "eigen3/Eigen/Core"
 #include "eigen3/Eigen/Geometry"
-#include "motion_common/motion_common.hpp"
-#include "motion_common/trajectory_common.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/utils.h"
 #include "tf2_ros/buffer.h"
@@ -34,11 +32,11 @@
 
 #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
 #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
+#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
 
 #include <deque>
 #include <memory>
@@ -57,7 +55,6 @@ namespace trajectory_follower
 using autoware::common::types::bool8_t;
 using autoware::common::types::float64_t;
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
-namespace motion_common = ::autoware::motion::motion_common;
 
 /// \class PidLongitudinalController
 /// \brief The node class used for generating longitudinal control commands (velocity/acceleration)
@@ -87,10 +84,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
   };
   rclcpp::Node * node_;
   // ros variables
-  rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr
-    m_pub_slope;
-  rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr
-    m_pub_debug;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_slope;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug;
 
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
   rcl_interfaces::msg::SetParametersResult paramCallback(

control/trajectory_follower/package.xml

@@ -24,14 +24,12 @@
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_geometry</depend>
   <depend>autoware_auto_planning_msgs</depend>
-  <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>diagnostic_msgs</depend>
   <depend>diagnostic_updater</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
-  <depend>motion_common</depend>
   <depend>motion_utils</depend>
   <depend>osqp_interface</depend>
   <depend>rclcpp</depend>
@@ -40,6 +38,7 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>vehicle_info_util</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

control/trajectory_follower/src/longitudinal_controller_utils.cpp

@@ -14,7 +14,6 @@
 
 #include "trajectory_follower/longitudinal_controller_utils.hpp"
 
-#include "motion_common/motion_common.hpp"
 #include "tf2/LinearMath/Matrix3x3.h"
 #include "tf2/LinearMath/Quaternion.h"
 
@@ -66,8 +65,7 @@ float64_t calcStopDistance(
   const Pose & current_pose, const Trajectory & traj, const float64_t max_dist,
   const float64_t max_yaw)
 {
-  const std::experimental::optional<size_t> stop_idx_opt =
-    trajectory_common::searchZeroVelocityIndex(traj.points);
+  const auto stop_idx_opt = motion_utils::searchZeroVelocityIndex(traj.points);
 
   const size_t end_idx = stop_idx_opt ? *stop_idx_opt : traj.points.size() - 1;
   const size_t seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
@@ -142,7 +140,7 @@ Pose calcPoseAfterTimeDelay(
   const Pose & current_pose, const float64_t delay_time, const float64_t current_vel)
 {
   // simple linear prediction
-  const float64_t yaw = ::motion::motion_common::to_angle(current_pose.orientation);
+  const float64_t yaw = tf2::getYaw(current_pose.orientation);
   const float64_t running_distance = delay_time * current_vel;
   const float64_t dx = running_distance * std::cos(yaw);
   const float64_t dy = running_distance * std::sin(yaw);

control/trajectory_follower/src/mpc.cpp

@@ -36,14 +36,13 @@ namespace control
 namespace trajectory_follower
 {
 using namespace std::literals::chrono_literals;
-using ::motion::motion_common::to_angle;
 
 bool8_t MPC::calculateMPC(
   const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer,
   const float64_t current_velocity, const geometry_msgs::msg::Pose & current_pose,
   autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd,
   autoware_auto_planning_msgs::msg::Trajectory & predicted_traj,
-  autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic)
+  tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic)
 {
   /* recalculate velocity from ego-velocity with dynamics */
   trajectory_follower::MPCTrajectory reference_trajectory =
@@ -133,9 +132,9 @@ bool8_t MPC::calculateMPC(
   const float64_t steer_cmd = ctrl_cmd.steering_tire_angle;
   const float64_t wb = m_vehicle_model_ptr->getWheelbase();
 
-  typedef decltype(diagnostic.diag_array.data)::value_type DiagnosticValueType;
+  typedef decltype(diagnostic.data)::value_type DiagnosticValueType;
   auto append_diag_data = [&](const auto & val) -> void {
-    diagnostic.diag_array.data.push_back(static_cast<DiagnosticValueType>(val));
+    diagnostic.data.push_back(static_cast<DiagnosticValueType>(val));
   };
   // [0] final steering command (MPC + LPF)
   append_diag_data(steer_cmd);
@@ -150,9 +149,9 @@ bool8_t MPC::calculateMPC(
   // [5] lateral error
   append_diag_data(mpc_data.lateral_err);
   // [6] current_pose yaw
-  append_diag_data(to_angle(current_pose.orientation));
+  append_diag_data(tf2::getYaw(current_pose.orientation));
   // [7] nearest_pose yaw
-  append_diag_data(to_angle(mpc_data.nearest_pose.orientation));
+  append_diag_data(tf2::getYaw(mpc_data.nearest_pose.orientation));
   // [8] yaw error
   append_diag_data(mpc_data.yaw_err);
   // [9] reference velocity
@@ -280,7 +279,7 @@ bool8_t MPC::getData(
   data->lateral_err =
     trajectory_follower::MPCUtils::calcLateralError(current_pose, data->nearest_pose);
   data->yaw_err = autoware::common::helper_functions::wrap_angle(
-    to_angle(current_pose.orientation) - to_angle(data->nearest_pose.orientation));
+    tf2::getYaw(current_pose.orientation) - tf2::getYaw(data->nearest_pose.orientation));
 
   /* get predicted steer */
   if (!m_steer_prediction_prev) {

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -155,9 +155,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
 
   m_pub_predicted_traj = node_->create_publisher<autoware_auto_planning_msgs::msg::Trajectory>(
     "~/output/predicted_trajectory", 1);
-  m_pub_diagnostic =
-    node_->create_publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>(
-      "~/output/lateral_diagnostic", 1);
+  m_pub_debug_values = node_->create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
+    "~/output/lateral_diagnostic", 1);
 
   // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations
   declareMPCparameters();
@@ -183,7 +182,7 @@ boost::optional<LateralOutput> MpcLateralController::run()
 
   autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd;
   autoware_auto_planning_msgs::msg::Trajectory predicted_traj;
-  autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic diagnostic;
+  tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values;
 
   if (!m_is_ctrl_cmd_prev_initialized) {
     m_ctrl_cmd_prev = getInitialControlCommand();
@@ -192,10 +191,10 @@ boost::optional<LateralOutput> MpcLateralController::run()
 
   const bool8_t is_mpc_solved = m_mpc.calculateMPC(
     *m_current_steering_ptr, m_current_kinematic_state_ptr->twist.twist.linear.x,
-    m_current_kinematic_state_ptr->pose.pose, ctrl_cmd, predicted_traj, diagnostic);
+    m_current_kinematic_state_ptr->pose.pose, ctrl_cmd, predicted_traj, debug_values);
 
   publishPredictedTraj(predicted_traj);
-  publishDiagnostic(diagnostic);
+  publishDebugValues(debug_values);
 
   const auto createLateralOutput = [this](const auto & cmd) {
     LateralOutput output;
@@ -391,12 +390,11 @@ void MpcLateralController::publishPredictedTraj(
   m_pub_predicted_traj->publish(predicted_traj);
 }
 
-void MpcLateralController::publishDiagnostic(
-  autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic & diagnostic) const
+void MpcLateralController::publishDebugValues(
+  tier4_debug_msgs::msg::Float32MultiArrayStamped & debug_values) const
 {
-  diagnostic.diag_header.data_stamp = node_->now();
-  diagnostic.diag_header.name = std::string("linear-MPC lateral controller");
-  m_pub_diagnostic->publish(diagnostic);
+  debug_values.stamp = node_->now();
+  m_pub_debug_values->publish(debug_values);
 }
 
 void MpcLateralController::declareMPCparameters()

control/trajectory_follower/src/mpc_utils.cpp

@@ -15,6 +15,7 @@
 #include "trajectory_follower/mpc_utils.hpp"
 
 #include "motion_utils/motion_utils.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
 #include <algorithm>
 #include <limits>
@@ -56,7 +57,7 @@ float64_t calcLateralError(
 {
   const float64_t err_x = ego_pose.position.x - ref_pose.position.x;
   const float64_t err_y = ego_pose.position.y - ref_pose.position.y;
-  const float64_t ref_yaw = ::motion::motion_common::to_angle(ref_pose.orientation);
+  const float64_t ref_yaw = tf2::getYaw(ref_pose.orientation);
   const float64_t lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y;
   return lat_err;
 }
@@ -232,7 +233,7 @@ bool8_t convertToMPCTrajectory(
     const float64_t x = p.pose.position.x;
     const float64_t y = p.pose.position.y;
     const float64_t z = 0.0;
-    const float64_t yaw = ::motion::motion_common::to_angle(p.pose.orientation);
+    const float64_t yaw = tf2::getYaw(p.pose.orientation);
     const float64_t vx = p.longitudinal_velocity_mps;
     const float64_t k = 0.0;
     const float64_t t = 0.0;
@@ -252,7 +253,7 @@ bool8_t convertToAutowareTrajectory(
     p.pose.position.x = static_cast<Real>(input.x.at(i));
     p.pose.position.y = static_cast<Real>(input.y.at(i));
     p.pose.position.z = static_cast<Real>(input.z.at(i));
-    p.pose.orientation = ::motion::motion_common::from_angle(input.yaw.at(i));
+    p.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(input.yaw.at(i));
     p.longitudinal_velocity_mps =
       static_cast<decltype(p.longitudinal_velocity_mps)>(input.vx.at(i));
     output.points.push_back(p);