diff --git a/config/multirotor_simulator.yaml b/config/multirotor_simulator.yaml index 9c69c9b..6e7bac6 100644 --- a/config/multirotor_simulator.yaml +++ b/config/multirotor_simulator.yaml @@ -2,6 +2,9 @@ simulation_rate: 100.0 # Hz clock_rate: 100.0 # Hz realtime_factor: 1.0 # [-] +# when true, the simulation time will get initialized from wall time +sim_time_from_wall_time: true + g: 9.81 # [ms^-2] iterate_without_input: true diff --git a/src/multirotor_simulator.cpp b/src/multirotor_simulator.cpp index 95909ba..412c4cb 100644 --- a/src/multirotor_simulator.cpp +++ b/src/multirotor_simulator.cpp @@ -106,9 +106,6 @@ void MultirotorSimulator::onInit() { srand(time(NULL)); - sim_time_ = ros::Time(0); - last_published_time_ = ros::Time(0); - mrs_lib::ParamLoader param_loader(nh_, "MultirotorSimulator"); std::string custom_config_path; @@ -132,6 +129,18 @@ void MultirotorSimulator::onInit() { double clock_rate; param_loader.loadParam("clock_rate", clock_rate); + bool sim_time_from_wall_time; + param_loader.loadParam("sim_time_from_wall_time", sim_time_from_wall_time); + + if (sim_time_from_wall_time) { + sim_time_ = ros::Time(ros::WallTime::now().toSec()); + } else { + sim_time_ = ros::Time(0); + } + + last_published_time_ = sim_time_; + last_sim_time_status_ = sim_time_; + drs_params_.paused = false; std::vector uav_names;