From 62a72841ab9a9e2ce0e8b678491089315dc44805 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 10 Sep 2023 03:53:09 +0900 Subject: [PATCH] update Signed-off-by: Takayuki Murooka --- .../simple_planning_simulator_core.hpp | 1 + .../simple_planning_simulator_core.cpp | 11 +++++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 43709c6ddd0a5..7798995344c21 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/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 diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index c50d54ef10b8a..46429711eaac9 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/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("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::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 {