update

Signed-off-by: Takayuki Murooka <[email protected]>

simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

@@ -185,6 +185,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   Trajectory::ConstSharedPtr current_trajectory_ptr_;
   bool simulate_motion_;  //!< stop vehicle motion simulation if false
   ControlModeReport current_control_mode_;
+  bool consider_ego_pitch_angle_;
 
   /* frame_id */
   std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -100,6 +100,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
   origin_frame_id_ = declare_parameter("origin_frame_id", "odom");
   add_measurement_noise_ = declare_parameter("add_measurement_noise", false);
   simulate_motion_ = declare_parameter<bool>("initial_engage_state");
+  consider_ego_pitch_angle_ = declare_parameter("consider_ego_pitch_angle", false);
 
   using rclcpp::QoS;
   using std::placeholders::_1;
@@ -287,7 +288,8 @@ double SimplePlanningSimulator::calculate_ego_pitch() const
 
   // calculate prev/next point of lanelet centerline nearest to ego pose.
   lanelet::Lanelet ego_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, ego_pose, &ego_lanelet)) {
+  if (!lanelet::utils::query::getClosestLaneletWithConstrains(
+        road_lanelets_, ego_pose, &ego_lanelet, 2.0, std::numeric_limits<double>::max())) {
     return 0.0;
   }
   const auto centerline_points = convert_centerline_to_points(ego_lanelet);
@@ -305,7 +307,9 @@ double SimplePlanningSimulator::calculate_ego_pitch() const
   const double diff_z = next_point.z - prev_point.z;
   const double diff_xy = std::hypot(next_point.x - prev_point.x, next_point.y - prev_point.y) /
                          std::cos(ego_yaw_against_lanelet);
-  const double ego_pitch_angle = std::atan2(diff_z, diff_xy);
+  const bool reverse_sign = std::cos(ego_yaw_against_lanelet) < 0.0;
+  const double ego_pitch_angle =
+    reverse_sign ? std::atan2(-diff_z, -diff_xy) : std::atan2(diff_z, diff_xy);
   return ego_pitch_angle;
 }
 
@@ -318,8 +322,7 @@ void SimplePlanningSimulator::on_timer()
 
   // calculate longitudinal acceleration by slope
   const double ego_pitch_angle = calculate_ego_pitch();
-  const double acc_by_slope = false ? 0.0 : -9.81 * std::sin(ego_pitch_angle);
-  std::cerr << ego_pitch_angle << std::endl;
+  const double acc_by_slope = consider_ego_pitch_angle_ ? -9.81 * std::sin(ego_pitch_angle) : 0.0;
 
   // update vehicle dynamics
   {