From f3e105c32e267d634b16859e8cfbbedaf796f749 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 19 Nov 2025 15:44:50 -0500 Subject: [PATCH 1/3] Remove the forked ros2 control node --- CMakeLists.txt | 10 -- src/mujoco_ros2_control_node.cpp | 192 ------------------------------- 2 files changed, 202 deletions(-) delete mode 100644 src/mujoco_ros2_control_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 461a9b6..d0c6878 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -177,16 +177,6 @@ install(TARGETS mujoco_ros2_simulation LIBRARY DESTINATION lib/${PROJECT_NAME} ) -# Install the ROS 2 control node -add_executable(ros2_control_node src/mujoco_ros2_control_node.cpp) -target_link_libraries(ros2_control_node PUBLIC - controller_manager::controller_manager -) -install( - TARGETS ros2_control_node - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - # Install python tools install(PROGRAMS scripts/find_missing_inertias.py diff --git a/src/mujoco_ros2_control_node.cpp b/src/mujoco_ros2_control_node.cpp deleted file mode 100644 index cbd8ea9..0000000 --- a/src/mujoco_ros2_control_node.cpp +++ /dev/null @@ -1,192 +0,0 @@ -// Copyright 2020 ROS2-Control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include - -#include -#include -#include - -using namespace std::chrono_literals; - -namespace -{ -// Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html -// This value is used when configuring the main loop to use SCHED_FIFO scheduling -// We use a midpoint RT priority to allow maximum flexibility to users -int const kSchedPriority = 50; - -} // namespace - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - std::shared_ptr executor = - std::make_shared(); - std::string manager_node_name = "controller_manager"; - - rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); - std::vector node_arguments = cm_node_options.arguments(); - for (int i = 1; i < argc; ++i) - { - if (node_arguments.empty() && std::string(argv[i]) != "--ros-args") - { - // A simple way to reject non ros args - continue; - } - node_arguments.push_back(argv[i]); - } - cm_node_options.arguments(node_arguments); - - auto cm = std::make_shared( - executor, manager_node_name, "", cm_node_options); - - const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); - - const bool has_realtime = realtime_tools::has_realtime_kernel(); - const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); - if (lock_memory) - { - const auto lock_result = realtime_tools::lock_memory(); - if (!lock_result.first) - { - RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory: '%s'", lock_result.second.c_str()); - } - } - - RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); - const bool manage_overruns = cm->get_parameter_or("overruns.manage", true); - RCLCPP_INFO( - cm->get_logger(), "Overruns handling is : %s", manage_overruns ? "enabled" : "disabled"); - const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); - RCLCPP_INFO( - cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(), - thread_priority); - - std::thread cm_thread( - [cm, thread_priority, use_sim_time, manage_overruns]() - { - rclcpp::Parameter cpu_affinity_param; - if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) - { - std::vector cpus = {}; - if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) - { - cpus = {static_cast(cpu_affinity_param.as_int())}; - } - else if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) - { - const auto cpu_affinity_param_array = cpu_affinity_param.as_integer_array(); - std::for_each( - cpu_affinity_param_array.begin(), cpu_affinity_param_array.end(), - [&cpus](int cpu) { cpus.push_back(static_cast(cpu)); }); - } - const auto affinity_result = realtime_tools::set_current_thread_affinity(cpus); - if (!affinity_result.first) - { - RCLCPP_WARN( - cm->get_logger(), "Unable to set the CPU affinity : '%s'", - affinity_result.second.c_str()); - } - } - - if (!realtime_tools::configure_sched_fifo(thread_priority)) - { - RCLCPP_WARN( - cm->get_logger(), - "Could not enable FIFO RT scheduling policy: with error number <%i>(%s). See " - "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " - "for details on how to enable realtime scheduling.", - errno, strerror(errno)); - } - else - { - RCLCPP_INFO( - cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.", - thread_priority); - } - - // CHANGED FROM UPSTREAM - // We must wait for the clock to be running, which requires the simulation to have started so that the - // physics loop will publish to the clock topic. To account for that, we must spin the executor - // and the controller manager node to ensure the MuJoCo hardware interface is constructed and launched. - // So instead, we just wait in the control loop so that the hardware interface can still start and run. - // - // TODO: Potentially remove this node depending on what comes out of the upstream PR: - // https://github.com/ros-controls/ros2_control/pull/2654 - cm->get_clock()->wait_until_started(); - cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate())); - - // for calculating sleep time - auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); - - // for calculating the measured period of the loop - rclcpp::Time previous_time = cm->get_trigger_clock()->now(); - std::this_thread::sleep_for(period); - - std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()}; - - while (rclcpp::ok()) - { - // calculate measured period - auto const current_time = cm->get_trigger_clock()->now(); - auto const measured_period = current_time - previous_time; - previous_time = current_time; - - // execute update loop - cm->read(cm->get_trigger_clock()->now(), measured_period); - cm->update(cm->get_trigger_clock()->now(), measured_period); - cm->write(cm->get_trigger_clock()->now(), measured_period); - - // wait until we hit the end of the period - if (use_sim_time) - { - cm->get_clock()->sleep_until(current_time + period); - } - else - { - next_iteration_time += period; - const auto time_now = std::chrono::steady_clock::now(); - if (manage_overruns && next_iteration_time < time_now) - { - const double time_diff = - static_cast( - std::chrono::duration_cast(time_now - next_iteration_time) - .count()) / - 1.e6; - const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); - const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); - RCLCPP_WARN_THROTTLE( - cm->get_logger(), *cm->get_clock(), 1000, - "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop " - "took %f ms (missed cycles : %d).", - cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); - next_iteration_time += (overrun_count * period); - } - std::this_thread::sleep_until(next_iteration_time); - } - } - }); - - executor->add_node(cm); - executor->spin(); - cm_thread.join(); - rclcpp::shutdown(); - return 0; -} From 2cf338affa34c4f7719cb44aeea70b0f87b46484 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 19 Nov 2025 15:44:59 -0500 Subject: [PATCH 2/3] Update README --- README.md | 20 -------------------- 1 file changed, 20 deletions(-) diff --git a/README.md b/README.md index 151a1b0..773c2c3 100644 --- a/README.md +++ b/README.md @@ -75,26 +75,6 @@ Just specify the plugin and point to a valid MJCF on launch: ... ``` -Due to compatibility issues, we use a [slightly modified ROS 2 control node](./src/mujoco_ros2_control_node.cpp). -It is the same executable and parameters as the upstream, but requires updating the launchfile: - -```python - control_node = Node( - # Specify the control node from this package! - package="mujoco_ros2_simulation", - executable="ros2_control_node", - output="both", - parameters=[ - {"use_sim_time": True}, - controller_parameters, - ], - ) -``` - -> **_NOTE_**: We can remove the the ROS 2 control node after the next ros2_control upstream release, -as the simulation requires [this PR](https://github.com/ros-controls/ros2_control/pull/2654) to run. -The hardware interface _should_ then be compatible with `humble`, `jazzy`, and `kilted`. - ### Joints Joints in the ros2_control interface are mapped to actuators defined in the MJCF. From 8b0ded0fe158f7858b8f5bf58682c03472fa88ce Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 19 Nov 2025 16:04:30 -0500 Subject: [PATCH 3/3] Revert to the controller_manager node --- test/launch/test_robot.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/launch/test_robot.launch.py b/test/launch/test_robot.launch.py index cc30370..875f265 100644 --- a/test/launch/test_robot.launch.py +++ b/test/launch/test_robot.launch.py @@ -60,7 +60,7 @@ def generate_launch_description(): ) control_node = Node( - package="mujoco_ros2_simulation", + package="controller_manager", executable="ros2_control_node", output="both", parameters=[