[ros2_control_node] Handle simulation environment clocks (#1810)

(cherry picked from commit d714e8b)

# Conflicts:
#	controller_manager/src/ros2_control_node.cpp
#	doc/release_notes.rst

controller_manager/src/ros2_control_node.cpp

@@ -42,7 +42,35 @@ int main(int argc, char ** argv)
     std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
   std::string manager_node_name = "controller_manager";
 
+<<<<<<< HEAD
   auto cm = std::make_shared<controller_manager::ControllerManager>(executor, manager_node_name);
+=======
+  rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options();
+  std::vector<std::string> 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<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());
+
+  const bool lock_memory = cm->get_parameter_or<bool>("lock_memory", true);
+  std::string message;
+  if (lock_memory && !realtime_tools::lock_memory(message))
+  {
+    RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str());
+  }
+>>>>>>> d714e8b ([ros2_control_node] Handle simulation environment clocks (#1810))
 
   const int cpu_affinity = cm->get_parameter_or<int>("cpu_affinity", -1);
   if (cpu_affinity >= 0)
@@ -68,7 +96,7 @@ int main(int argc, char ** argv)
     thread_priority);
 
   std::thread cm_thread(
-    [cm, thread_priority]()
+    [cm, thread_priority, use_sim_time, &rate]()
     {
       if (realtime_tools::has_realtime_kernel())
       {
@@ -120,7 +148,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);
+        }
       }
     });
 

doc/release_notes.rst

@@ -16,6 +16,50 @@ controller_interface
 controller_manager
 ******************
 
+<<<<<<< HEAD
+=======
+  *  Configured chainable controller: Listed exported interfaces are unavailable and unclaimed
+  *  Active chainable controller (not in chained mode): Listed exported interfaces are available but unclaimed
+  *  Active chainable controller (in chained mode): Listed exported interfaces are available and claimed
+* Try using SCHED_FIFO on any kernel (`#1142 <https://github.com/ros-controls/ros2_control/pull/1142>`_)
+* A method to get node options to setup the controller node was added (`#1169 <https://github.com/ros-controls/ros2_control/pull/1169>`_): ``get_node_options`` can be overridden by controllers, this would make it easy for other controllers to be able to setup their own custom node options
+* CM now subscribes to ``robot_description`` topic instead of ``~/robot_description`` (`#1410 <https://github.com/ros-controls/ros2_control/pull/1410>`_).
+* Change the controller sorting with an approach similar to directed acyclic graphs (`#1384 <https://github.com/ros-controls/ros2_control/pull/1384>`_)
+* Changes from `(PR #1256) <https://github.com/ros-controls/ros2_control/pull/1256>`__
+
+  * All ``joints`` defined in the ``<ros2_control>``-tag have to be present in the URDF received :ref:`by the controller manager <doc/ros2_control/controller_manager/doc/userdoc:subscribers>`, otherwise the following error is shown:
+
+      The published robot description file (URDF) seems not to be genuine. The following error was caught: <unknown_joint> not found in URDF.
+
+    This is to ensure that the URDF and the ``<ros2_control>``-tag are consistent. E.g., for configuration ports use ``gpio`` interface types instead.
+
+  * The syntax for mimic joints is changed to the `official URDF specification <https://wiki.ros.org/urdf/XML/joint>`__.
+
+    .. code-block:: xml
+
+      <joint name="right_finger_joint" type="prismatic">
+        <axis xyz="0 1 0"/>
+        <origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
+        <parent link="base"/>
+        <child link="finger_right"/>
+        <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+      </joint>
+      <joint name="left_finger_joint" type="prismatic">
+        <mimic joint="right_finger_joint" multiplier="1" offset="0"/>
+        <axis xyz="0 1 0"/>
+        <origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
+        <parent link="base"/>
+        <child link="finger_left"/>
+        <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+      </joint>
+
+   The parameters within the ``ros2_control`` tag are not supported any more.
+* The support for the ``description`` parameter for loading the URDF was removed (`#1358 <https://github.com/ros-controls/ros2_control/pull/1358>`_).
+* The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 <https://github.com/ros-controls/ros2_control/pull/1639>`_).
+* The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 <https://github.com/ros-controls/ros2_control/pull/1640>`_).
+* Added support for the wildcard entries for the controller configuration files (`#1724 <https://github.com/ros-controls/ros2_control/pull/1724>`_).
+* ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 <https://github.com/ros-controls/ros2_control/pull/1810>`_).
+>>>>>>> d714e8b ([ros2_control_node] Handle simulation environment clocks (#1810))
 * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 <https://github.com/ros-controls/ros2_control/pull/1820>`_).
 * Added support for the wildcard entries for the controller configuration files (`#1724 <https://github.com/ros-controls/ros2_control/pull/1724>`_).
 * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 <https://github.com/ros-controls/ros2_control/pull/1822>`_).