Have any flag from Gazebo or ROS can make the simulation deterministic? Or can it only be customized by hand?

asked 2019-12-19 00:59:25 -0500

FH gravatar image

updated 2019-12-22 21:32:51 -0500

Hi,

Does anyone know whether exist a flag or parameter in ROS or Gazebo can make the simulation avoid message drop? Meaning, all messages in specify topic would definitely process in the subscriber node.

Platform:

  • Ubuntu 16.04

  • ROS: Kinetic

  • Gazebo: 7

Currently, I make an experiment to avoid message drop and try to perform like a "deterministic" manner. The following are some features:

  1. Using the turtlebot (kobuki) in simulation.
  2. In Gazebo .world file, max_step_size=0.001; real_time_factor=1; real_time_update_rate=1000;

  3. Publisher node publishes (with 1000 Hz) odometry message to /odom topic ( with queue size = 1). Actually, the publisher node is the gazebo node and using the plugin to publish odometry message.

  4. Subscriber node subscribes /odom topic (with queue size = 10).
  5. need to pause/unpause the simulation to wait for the processing of callback function. In my experiment, I let it sleep enough to wait to process.

The following is my code:

#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Header.h>
#include <nav_msgs/Odometry.h>

bool g_unpause = true;

bool pauseWorld() {
    ros::NodeHandle n;
    ros::ServiceClient pause = n.serviceClient<std_srvs::Empty>("/gazebo/pause_physics");
    std_srvs::Empty empty;
    pause.call(empty);
    return true;
}

bool unpauseWorld() {
    ros::NodeHandle n;
    ros::ServiceClient unpause = n.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics");
    std_srvs::Empty empty;
    unpause.call(empty);
    return true;
}

class OdomListener {
    ros::NodeHandle nh_;
    ros::Subscriber sub_;

    double last_time_;
    double cur_time_;
    double dt_;

    int last_seq_;
    int cur_seq_;
    int dseq_;
    public:
    OdomListener():last_seq_(-1),last_time_(-1) {
            sub_ = nh_.subscribe("odom",10,&OdomListener::waitConsumerCB,this);
    }

    void waitConsumerCB(const nav_msgs::Odometry::ConstPtr& msg) {
            g_unpause = false;

            cur_time_ = msg->header.stamp.toSec();
            cur_seq_ = msg->header.seq;

            if (last_time_ > 0) {
              dt_ = cur_time_ - last_time_;
              dseq_ = cur_seq_ - last_seq_;
              if (dt_ >= 0.002) { // in the header.stamp jump case
                ROS_INFO_STREAM(" == msgs time step : " << cur_time_ << " " << last_time_ << " " << dt_);
              }

              if (dseq_ >= 2) { // in the header.seq jump case
                ROS_INFO_STREAM(" == msgs seq step : " << cur_seq_ << " " << last_seq_ << " " << dseq_);
              }

              ROS_INFO_STREAM(" ============== Just the Flag ================= ");
            }

            last_time_ = cur_time_;
            last_seq_ = cur_seq_;
    }
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "odom_listener");

    OdomListener ol;

    while(ros::ok()) {
            if (g_unpause) {
                    //ROS_INFO("In unpause");
                    unpauseWorld();
            }

            if (!g_unpause) {
                    //ROS_INFO("In pause");
                    pauseWorld();
                    ros::WallDuration(3).sleep();
                    g_unpause = true;
            }
            ros::spinOnce();

    }

    return 0;
}

Finally, even it works but I think this not robust because it still needs to consider the frequency of message and subscriber queue in each case and so on.

Also, what if the situation going to be sophistication. Like leverage move_base node to do navigation, is it appropriate to integrate into this case? How do I know when I can unpause the simulation?

Or in multiple subscribers node case. Because it needs to consider the unpause condition (need to monitor all subscribers node whether finishing their processing or not), right?

I will be very thankful if anyone can help me =)

edit retag flag offensive close merge delete

Comments

This is probably better asked at http://answers.gazebosim.org/questions/

ct2034 gravatar image ct2034  ( 2019-12-19 12:42:25 -0500 )edit