Skip to content

Commit

Permalink
update
Browse files Browse the repository at this point in the history
Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
takayuki5168 committed Sep 11, 2023
1 parent ee9c27c commit 343b340
Show file tree
Hide file tree
Showing 2 changed files with 4 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -185,7 +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_;
bool enable_road_slope_simulation_;

/* frame_id */
std::string simulated_frame_id_; //!< @brief simulated vehicle frame id
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,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);
enable_road_slope_simulation_ = declare_parameter("enable_road_slope_simulation", false);

using rclcpp::QoS;
using std::placeholders::_1;
Expand Down Expand Up @@ -324,7 +324,8 @@ void SimplePlanningSimulator::on_timer()

// calculate longitudinal acceleration by slope
const double ego_pitch_angle = calculate_ego_pitch();
const double acc_by_slope = consider_ego_pitch_angle_ ? -9.81 * std::sin(ego_pitch_angle) : 0.0;
const double acc_by_slope =
enable_road_slope_simulation_ ? -9.81 * std::sin(ego_pitch_angle) : 0.0;

// update vehicle dynamics
{
Expand Down

0 comments on commit 343b340

Please sign in to comment.