(Guide) How to set real time factor in ROS2 communications. (How to modify sync time like Webots or Gazebo).

Turned out to be very simple. But first, why did I want to modify the real time factor in the first place? I am using MATLAB to send commands to ROS2 which sends these commands (custom torque messages -check the first guide-) to Stonefish simulator. Designing even a simple controller will add delays and these delays are interpreted in the simulation as 0 values, which produced instability and very high oscillations. I originally thought about implementing for loops in the servo but they didn't work since the delays are not consistent and I couldn't find an actual way to measure the process time for ROS2 to stonefish (and even if I did, I need a variable for the 'for-loop' and it's complicated).

So, I brainstormed and came up with three solutions: 1- add joint damping (friction) introduced in the last guide. 2- parrallel computing in MATLAB (which I think is the best option, but didn't try it yet). 3- make the simulation slower but STEP (or ADVANCE) the simulation physics with the same "time step".

Solution 3 was a little tricky as I kept changing the realtimefactor in the original stonefish package but it never worked. Turns out that ROS2 package is overriding the simulation clock, so with a simple edit:

file "ROS2SimulationManager.cpp" ---> line 82 edit the void "ROS2SimulationManager::getSimulationClock()"

uint64_t ROS2SimulationManager::getSimulationClock() const
{
    rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now();
    return ceil( customRTF * (now.nanoseconds()/1000) );
}

Where customRTF is a value of your choosing.

Noting that void "ROS2SimulationManager::SimulationClockSleep(uint64_t us)" also needs to be modified, but I don't understand its function yet, so I will leave it as it's and will update this comment later.

patrykcieslak / stonefish_ros2

(Guide) How to set real time factor in ROS2 communications. (How to modify sync time like Webots or Gazebo). #2