ROS control: rate.sleep() blocks in rostest

asked 2021-03-22 15:53:42 -0600

fjp gravatar image

Hi, I am using the diff_drive_controller from ROS control on a real robot hardware, which is working.

When I try to use the hardware interface and control loop in my rostest (based on the tests in diff_drive_controller), the execution seems to get stuck in the call to rate.sleep() at the end of the while(ros::ok()) loop:

#include <ros/ros.h>
#include <diffbot_base/diffbot_hw_interface.h>
#include <controller_manager/controller_manager.h>

int main(int argc, char **argv)
{
    // Initialize the ROS node
    ros::init(argc, argv, "diffbot_hw_interface");
    ros::NodeHandle nh;

    // Setup a separate thread that will be used to service ROS callbacks.
    // NOTE: We run the ROS loop in a separate thread as external calls such
    // as service callbacks to load controllers can block the (main) control loop
    ros::AsyncSpinner spinner(3);
    spinner.start();

    // Create an instance of your robot so that this instance knows about all 
    // the resources that are available.
    diffbot_base::DiffBotHWInterface diffBot(nh);

    // Create an instance of the controller manager and pass it the robot, 
    // so that it can handle its resources.
    controller_manager::ControllerManager cm(&diffBot);


    // Setup for the control loop.
    ros::Time prev_time = ros::Time::now();
    ros::Rate rate(10.0); // 50 Hz rate

    // Blocks until shutdown signal recieved
    while (ros::ok())
    {
        // Basic bookkeeping to get the system time in order to compute the control period.
        const ros::Time     time   = ros::Time::now();
        const ros::Duration period = time - prev_time;
        prev_time = time;

        // Execution of the actual control loop.
        diffBot.read(time, period);
        // If needed, its possible to define transmissions in software by calling the 
        // transmission_interface::ActuatorToJointPositionInterface::propagate()
        // after reading the joint states.
        cm.update(time, period);
        // In case of software transmissions, use 
        // transmission_interface::JointToActuatorEffortHandle::propagate()
        // to convert from the joint space to the actuator space.
        diffBot.write(time, period);

        //ros::spinOnce();

        // All these steps keep getting repeated with the specified rate.
        ROS_INFO_STREAM("BEFORE");
        rate.sleep();
        ROS_INFO_STREAM("AFTER");
    }
    return 0;
}

This loop is basically from the original ros controls documentation mentioned in the ROSCon 2014 talk and doxygen docs.

In the above code, when running the rostest, I see the "BEFORE" output but not the "AFTER". Also the calls to diffBot.read() and diffBot.write() log only once.

What is the reason that this loop cycles continuously on the real robot hardware but blocks in the rostest?

Is this problem related to the timing issues mentioned in https://groups.google.com/g/ros-sig-r... and https://github.com/PickNikRobotics/ro... ? Should I work with clock_gettime(CLOCK_MONOTONIC, &current_time); or std::chrono::system_clock::now(); instead of just ros::Time::now()? If so, what are the problems that might happen when using just ros::Time::now() like I have it done currently to compute the period?

edit retag flag offensive close merge delete

Comments

What is the reason that this loop cycles continuously on the real robot hardware but blocks in the rostest?

Just found out that the blocking inside rate.sleep() happens because of use_sim_time = true, which is set in the common test launch file.

When I don't set use_sim_time, then the loop iterates continuously, but I guess it would be better to work with the /clock topic in tests. Like it is done in your tests, e.g. here in diffbot.cpp.

fjp gravatar image fjp  ( 2021-03-22 18:21:51 -0600 )edit

Regarding my second question, I would still be interested to know, if using only ros::Time::now() should be avoided and an implementation similar to PickNikRobotics/ros_control_boilerplate is the recommended way? Thank

fjp gravatar image fjp  ( 2021-03-22 18:21:57 -0600 )edit