Merge branch 'main' into add-data-path-arg-to-perception-launch

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -1850,7 +1850,8 @@ insertOrientation<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>
 
 /**
  * @brief Remove points with invalid orientation differences from a given points container
- * (trajectory, path, ...)
+ * (trajectory, path, ...). Check the difference between the angles of two points and the difference
+ * between the azimuth angle between the two points and the angle of the next point.
  * @param points Points of trajectory, path, or other point container (input / output)
  * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in
  * radians (default: M_PI_2)
@@ -1859,10 +1860,14 @@ template <class T>
 void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
 {
   for (size_t i = 1; i < points.size();) {
-    const double yaw1 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i - 1)).orientation);
-    const double yaw2 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i)).orientation);
-
-    if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2))) {
+    const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPose(points.at(i));
+    const double yaw1 = tf2::getYaw(p1.orientation);
+    const double yaw2 = tf2::getYaw(p2.orientation);
+
+    if (
+      max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) ||
+      !tier4_autoware_utils::isDrivingForward(p1, p2)) {
       points.erase(points.begin() + i);
     } else {
       ++i;

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

@@ -38,15 +38,10 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <tf2/utils.h>
 
-#ifdef ROS_DISTRO_GALACTIC
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#else
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#endif
-
 // TODO(wep21): Remove these apis
 //              after they are implemented in ros2 geometry2.
 namespace tf2

common/tier4_autoware_utils/src/geometry/geometry.cpp

@@ -16,7 +16,10 @@
 
 #include <Eigen/Geometry>
 
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
 #include <tf2/convert.h>
+
 namespace tf2
 {
 void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped<tf2::Transform> & out)

common/tier4_planning_rviz_plugin/include/path/display_base.hpp

@@ -133,7 +133,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
     property_point_alpha_{"Alpha", 1.0, "", &property_point_view_},
     property_point_color_{"Color", QColor(0, 60, 255), "", &property_point_view_},
     property_point_radius_{"Radius", 0.1, "", &property_point_view_},
-    property_point_offset_{"Offset", 0.0, "", &property_point_view_}
+    property_point_offset_{"Offset", 0.0, "", &property_point_view_},
+    // slope
+    property_slope_text_view_{"View Text Slope", false, "", this},
+    property_slope_text_scale_{"Scale", 0.3, "", &property_slope_text_view_}
   {
     // path
     property_path_width_.setMin(0.0);
@@ -171,6 +174,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
         node->detachAllObjects();
         this->scene_manager_->destroySceneNode(node);
       }
+      for (size_t i = 0; i < slope_text_nodes_.size(); i++) {
+        Ogre::SceneNode * node = slope_text_nodes_.at(i);
+        node->removeAndDestroyAllChildren();
+        node->detachAllObjects();
+        this->scene_manager_->destroySceneNode(node);
+      }
       this->scene_manager_->destroyManualObject(footprint_manual_object_);
       this->scene_manager_->destroyManualObject(point_manual_object_);
     }
@@ -198,6 +207,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
     rviz_common::MessageFilterDisplay<T>::MFDClass::reset();
     path_manual_object_->clear();
     velocity_manual_object_->clear();
+
     for (size_t i = 0; i < velocity_texts_.size(); i++) {
       Ogre::SceneNode * node = velocity_text_nodes_.at(i);
       node->detachAllObjects();
@@ -206,6 +216,16 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
     }
     velocity_text_nodes_.clear();
     velocity_texts_.clear();
+
+    for (size_t i = 0; i < slope_texts_.size(); i++) {
+      Ogre::SceneNode * node = slope_text_nodes_.at(i);
+      node->detachAllObjects();
+      node->removeAndDestroyAllChildren();
+      this->scene_manager_->destroySceneNode(node);
+    }
+    slope_text_nodes_.clear();
+    slope_texts_.clear();
+
     footprint_manual_object_->clear();
     point_manual_object_->clear();
   }
@@ -300,6 +320,29 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
       velocity_text_nodes_.resize(msg_ptr->points.size());
     }
 
+    if (msg_ptr->points.size() > slope_texts_.size()) {
+      for (size_t i = slope_texts_.size(); i < msg_ptr->points.size(); i++) {
+        Ogre::SceneNode * node = this->scene_node_->createChildSceneNode();
+        rviz_rendering::MovableText * text =
+          new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1);
+        text->setVisible(false);
+        text->setTextAlignment(
+          rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE);
+        node->attachObject(text);
+        slope_texts_.push_back(text);
+        slope_text_nodes_.push_back(node);
+      }
+    } else if (msg_ptr->points.size() < slope_texts_.size()) {
+      for (size_t i = slope_texts_.size() - 1; i >= msg_ptr->points.size(); i--) {
+        Ogre::SceneNode * node = slope_text_nodes_.at(i);
+        node->detachAllObjects();
+        node->removeAndDestroyAllChildren();
+        this->scene_manager_->destroySceneNode(node);
+      }
+      slope_texts_.resize(msg_ptr->points.size());
+      slope_text_nodes_.resize(msg_ptr->points.size());
+    }
+
     const auto info = vehicle_footprint_info_;
     const float left = property_path_width_view_.getBool() ? -property_path_width_.getFloat() / 2.0
                                                            : -info->width / 2.0;
@@ -395,6 +438,38 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
         rviz_rendering::MovableText * text = velocity_texts_.at(point_idx);
         text->setVisible(false);
       }
+
+      // slope text
+      if (property_slope_text_view_.getBool() && 1 < msg_ptr->points.size()) {
+        const size_t prev_idx =
+          (point_idx != msg_ptr->points.size() - 1) ? point_idx : point_idx - 1;
+        const size_t next_idx =
+          (point_idx != msg_ptr->points.size() - 1) ? point_idx + 1 : point_idx;
+
+        const auto & prev_path_pos =
+          tier4_autoware_utils::getPose(msg_ptr->points.at(prev_idx)).position;
+        const auto & next_path_pos =
+          tier4_autoware_utils::getPose(msg_ptr->points.at(next_idx)).position;
+
+        Ogre::Vector3 position;
+        position.x = pose.position.x;
+        position.y = pose.position.y;
+        position.z = pose.position.z;
+        Ogre::SceneNode * node = slope_text_nodes_.at(point_idx);
+        node->setPosition(position);
+
+        rviz_rendering::MovableText * text = slope_texts_.at(point_idx);
+        const double slope = tier4_autoware_utils::calcElevationAngle(prev_path_pos, next_path_pos);
+
+        std::stringstream ss;
+        ss << std::fixed << std::setprecision(2) << slope;
+        text->setCaption(ss.str());
+        text->setCharacterHeight(property_slope_text_scale_.getFloat());
+        text->setVisible(true);
+      } else {
+        rviz_rendering::MovableText * text = slope_texts_.at(point_idx);
+        text->setVisible(false);
+      }
     }
 
     path_manual_object_->end();
@@ -526,6 +601,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
   Ogre::ManualObject * point_manual_object_{nullptr};
   std::vector<rviz_rendering::MovableText *> velocity_texts_;
   std::vector<Ogre::SceneNode *> velocity_text_nodes_;
+  std::vector<rviz_rendering::MovableText *> slope_texts_;
+  std::vector<Ogre::SceneNode *> slope_text_nodes_;
 
   rviz_common::properties::BoolProperty property_path_view_;
   rviz_common::properties::BoolProperty property_path_width_view_;
@@ -556,6 +633,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
   rviz_common::properties::FloatProperty property_point_radius_;
   rviz_common::properties::FloatProperty property_point_offset_;
 
+  rviz_common::properties::BoolProperty property_slope_text_view_;
+  rviz_common::properties::FloatProperty property_slope_text_scale_;
+
   std::shared_ptr<VehicleInfo> vehicle_info_;
 
 private:

common/tier4_target_object_type_rviz_plugin/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>cv_bridge</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>

control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp

@@ -66,11 +66,6 @@ double getPitchByPose(const Quaternion & quaternion);
 double getPitchByTraj(
   const Trajectory & trajectory, const size_t closest_idx, const double wheel_base);
 
-/**
- * @brief calculate elevation angle
- */
-double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to);
-
 /**
  * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and
  * acceleration for delayed time

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -365,7 +365,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
    * @param [in] current_vel current velocity of the vehicle
    */
   double applyVelocityFeedback(
-    const Motion target_motion, const double dt, const double current_vel);
+    const Motion target_motion, const double dt, const double current_vel, const Shift & shift);
 
   /**
    * @brief update variables for debugging about pitch

control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp

@@ -91,41 +91,22 @@ double getPitchByTraj(
     return 0.0;
   }
 
-  for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) {
-    const double dist = tier4_autoware_utils::calcDistance2d(
-      trajectory.points.at(nearest_idx), trajectory.points.at(i));
-    if (dist > wheel_base) {
-      // calculate pitch from trajectory between rear wheel (nearest) and front center (i)
-      return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i));
+  const auto [prev_idx, next_idx] = [&]() {
+    for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) {
+      const double dist = tier4_autoware_utils::calcDistance2d(
+        trajectory.points.at(nearest_idx), trajectory.points.at(i));
+      if (dist > wheel_base) {
+        // calculate pitch from trajectory between rear wheel (nearest) and front center (i)
+        return std::make_pair(nearest_idx, i);
+      }
     }
-  }
-
-  // close to goal
-  for (size_t i = trajectory.points.size() - 1; i > 0; --i) {
-    const double dist =
-      tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i));
-
-    if (dist > wheel_base) {
-      // calculate pitch from trajectory
-      // between wheelbase behind the end of trajectory (i) and the end of trajectory (back)
-      return calcElevationAngle(trajectory.points.at(i), trajectory.points.back());
-    }
-  }
+    // NOTE: The ego pose is close to the goal.
+    return std::make_pair(
+      std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1);
+  }();
 
-  // calculate pitch from trajectory between the beginning and end of trajectory
-  return calcElevationAngle(trajectory.points.at(0), trajectory.points.back());
-}
-
-double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to)
-{
-  const double dx = p_from.pose.position.x - p_to.pose.position.x;
-  const double dy = p_from.pose.position.y - p_to.pose.position.y;
-  const double dz = p_from.pose.position.z - p_to.pose.position.z;
-
-  const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits<double>::epsilon());
-  const double pitch = std::atan2(dz, dxy);
-
-  return pitch;
+  return tier4_autoware_utils::calcElevationAngle(
+    trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position);
 }
 
 Pose calcPoseAfterTimeDelay(

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -531,10 +531,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s);
   };
 
-  // if current operation mode is not autonomous mode, then change state to stopped
-  if (m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) {
-    return changeState(ControlState::STOPPED);
-  }
+  const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
+                                m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
 
   // transit state
   // in DRIVE state
@@ -543,6 +541,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
       return changeState(ControlState::EMERGENCY);
     }
 
+    if (!is_under_control && stopped_condition && keep_stopped_condition) {
+      // NOTE: When the ego is stopped on manual driving, since the driving state may transit to
+      //       autonomous, keep_stopped_condition should be checked.
+      return changeState(ControlState::STOPPED);
+    }
+
     if (m_enable_smooth_stop) {
       if (stopping_condition) {
         // predictions after input time delay
@@ -609,8 +613,14 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
   // in EMERGENCY state
   if (m_control_state == ControlState::EMERGENCY) {
-    if (stopped_condition && !emergency_condition) {
-      return changeState(ControlState::STOPPED);
+    if (!emergency_condition) {
+      if (stopped_condition) {
+        return changeState(ControlState::STOPPED);
+      }
+      if (!is_under_control) {
+        // NOTE: On manual driving, no need stopping to exit the emergency.
+        return changeState(ControlState::DRIVE);
+      }
     }
     return;
   }
@@ -644,7 +654,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd(
     m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target);
 
     raw_ctrl_cmd.vel = target_motion.vel;
-    raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target);
+    raw_ctrl_cmd.acc =
+      applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift);
     RCLCPP_DEBUG(
       logger_,
       "[feedback control]  vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f "
@@ -817,7 +828,7 @@ double PidLongitudinalController::applySlopeCompensation(
   const double pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad);
 
   // Acceleration command is always positive independent of direction (= shift) when car is running
-  double sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0);
+  double sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0);
   double compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited);
   return compensated_acc;
 }
@@ -919,14 +930,16 @@ double PidLongitudinalController::predictedVelocityInTargetPoint(
 }
 
 double PidLongitudinalController::applyVelocityFeedback(
-  const Motion target_motion, const double dt, const double current_vel)
+  const Motion target_motion, const double dt, const double current_vel, const Shift & shift)
 {
-  const double current_vel_abs = std::fabs(current_vel);
-  const double target_vel_abs = std::fabs(target_motion.vel);
-  const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
+  // NOTE: Acceleration command is always positive even if the ego drives backward.
+  const double vel_sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0.0);
+  const double diff_vel = (target_motion.vel - current_vel) * vel_sign;
+  const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
+                                m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
   const bool enable_integration =
-    (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
-  const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs);
+    (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control;
+  const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel);
 
   std::vector<double> pid_contributions(3);
   const double pid_acc =
@@ -939,8 +952,8 @@ double PidLongitudinalController::applyVelocityFeedback(
   // deviation will be bigger.
   constexpr double ff_scale_max = 2.0;  // for safety
   constexpr double ff_scale_min = 0.5;  // for safety
-  const double ff_scale =
-    std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max);
+  const double ff_scale = std::clamp(
+    std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max);
   const double ff_acc = target_motion.acc * ff_scale;
 
   const double feedback_acc = ff_acc + pid_acc;