Fix Gazebo timer issue in sim-instr

[Elton-Lin]

• Problem: Twist messages are published instantly because callback function is executed before timer is created and started.
• Solution: Adding a while loop to ensure timer is created and valid before executing the callback. And the test file should be written in a time reversed order because the program takes lines by popping from the back of the list.

Now the sim_instr_node.cpp is able to take in a text file with times and twist messages and execute them the correct order in Gazebo simulation though the motion of the robot is still imperfect.

[Gregory-Meyer]

Shouldn't the file be reversed in the code, as opposed to by the user? Also, what happens if the user outputs a sequence of non-increasing timer events? e.g. The first line fires at 5 seconds, but the second line fires at 3 seconds.

[Elton-Lin]

Yes. I agree with you. Should I create another file and fix that problem or just do it on this current file although it was not written by me? And I guess I would just check if the time sequence is valid before running anything and exit the program if it is not. (any non-increasing sequence)

[Gregory-Meyer]

I think that a better solution would be to either check if the input is sorted, then notify the user if it has to be re-sorted. Alternatively, the callback function could hold a reference to its info, negating any need for ordering in the input (file or in-memory representation)

[Elton-Lin]

I don't quite understand how to implement "callback function could hold a reference to its info" to not worry about the order (I will look into that later). The other ways I can think of is mapping the times value to their corresponding twists messages; or re-sort the file in the program first. But I still think having the user to order their commands is better for them.

[Gregory-Meyer]

I don't quite understand how to implement "callback function could hold a reference to its info" to not worry about the order (I will look into that later).

Lambda functions can "capture" variables from the enclosing scope either by reference, value, or any other way you like. You just have to indicate as such in the lambda's capture list.

Capturing by value:

geometry_msgs::Twist twist;
const auto callback = [twist](const ros::TimerEvent&) {
    use_twist(twist);
};

modify(twist);
use_callback(callback); // will see twist as it was previously

Now callback has a copy of twist from the time it was created. This is pretty much identical to us doing:

struct Callback {
    geometry_msgs::Twist twist;

    void operator()(const ros::TimerEvent&) {
        use_twist(twist);
    }
};

const geometry_msgs::Twist twist;
const Callback callback = {twist};
modify(twist);
use_callback(callback); // will see twist as it was previously

Capturing by reference:

const geometry_msgs::Twist twist;
const auto callback = [&twist](const ros::TimerEvent&) {
    use_twist(twist);
};
modify(twist);
use_callback(callback); // will see twist as it currently exists

Which is the same as:

struct Callback {
    const geometry_msgs::Twist &twist;

    void operator()(const ros::TimerEvent&) {
        use_twist(twist);
    }
};

const geometry_msgs::Twist twist;
const Callback callback = {twist};
modify(twist);
use_callback(callback); // will see twist as it currently exists

While it's tempting to use references frequently, you should keep the lifetime of the captured variable in mind. You can't use lambdas to get around returning a reference to a local variable!

You can also move objects into lambdas:

std::vector<std::string> names = get_names();
const auto callback = [names = std::move(names)]() mutable {
    assert(!names.empty());

    const std::string name = names.back();
    names.pop_back();

    return name;
};

But this is more concisely expressed as:

const auto callback = [names = get_names()]() mutable {
    assert(!names.empty());

    const std::string name = names.back();
    names.pop_back();

    return name;
};

You can learn more about the gritty details of lambda expressions by browsing cppreference.

But I still think having the user to order their commands is better for them.

I agree, but IMO it shouldn't be a fatal error for that to happen. Maybe emit an error with ROS_WARN_STREAM and then proceed correctly regardless?