Skip to content

Commit

Permalink
make ros2_control_node handle simulation environments
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor committed Oct 24, 2024
1 parent d55def1 commit 2651b99
Showing 1 changed file with 11 additions and 2 deletions.
13 changes: 11 additions & 2 deletions controller_manager/src/ros2_control_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,12 @@ int main(int argc, char ** argv)
auto cm = std::make_shared<controller_manager::ControllerManager>(
executor, manager_node_name, "", cm_node_options);

const bool use_sim_time = cm->get_parameter_or("use_sim_time", false);
rclcpp::Rate rate(cm->get_update_rate(), cm->get_clock());
RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());

std::thread cm_thread(
[cm]()
[cm, use_sim_time, &rate]()
{
if (!realtime_tools::configure_sched_fifo(kSchedPriority))
{
Expand Down Expand Up @@ -101,7 +103,14 @@ int main(int argc, char ** argv)

// wait until we hit the end of the period
next_iteration_time += period;
std::this_thread::sleep_until(next_iteration_time);
if (use_sim_time)
{
rate.sleep();
}
else
{
std::this_thread::sleep_until(next_iteration_time);
}
}

cm->shutdown_async_controllers_and_components();
Expand Down

0 comments on commit 2651b99

Please sign in to comment.