Reporting artifacts

Issue #186 closed
Martin Dlouhy created an issue

I am reporting artifacts via this function:

  public: bool ReportArtifact(const uint32_t _type,
                               const ignition::msgs::Pose &_pose)
  {
    subt::msgs::Artifact artifact;
    artifact.set_type(_type);
    artifact.mutable_pose()->CopyFrom(_pose);
    return this->client->SendTo(artifact.SerializeAsString(), subt::kBaseStationName);
  }

And so far I am permanently getting “false” and server does not see any report attempt - any suggestion what I am doing wrong? I call start_scoring:

  public: bool start_scoring()
  {
    ignition::msgs::Boolean req;
    ignition::msgs::Boolean rep;
    unsigned int timeout = 5000;
    bool result;
    req.set_data(true);
    return this->node.Request("/subt/start", req, timeout, rep, result); 
  }

before that and it works fine

[Msg] Scoring has Started

Thanks in advance for any hit

Martin

Comments (44)

  1. Nate Koenig

    The comms model could be dropping your score attempt. What world are you running, and how are you launching simulation? When you report a score, how far from the base station is your robot?

  2. Martin Dlouhy reporter

    Hi Nate,

    thanks for coming back to this issue. I am running docker image

     ./run.bash robotika tunnel_circuit_practice.ign worldName:=tunnel_circuit_practice_01 robotName1:=X2 robotConfig1:=X2_SENSOR_CONFIG_3
    

    where robotika is based on downloaded FROM osrf/subt-virtual-testbed

    Inside I am trying to run modified e:\hg\subt\subt_example\src\subt_example_node.cc

    (extended for reporting artifact and communicating with our python3 code)

    and launched with roslaunch subt_example example_robot.launch where I changed name from X1 to X2.

    The robot enters tunnel and returns back to the base, so it is reported in the base area (BTW does it have to enter the tunnel, as it used to be, or for test is enough to start scoring?) The position of the artifact could be wrong.

  3. Nate Koenig

    Thanks for the info. The robot must get close enough to an artifact for it to be loaded before you can attempt to score with the artifact. You can see parts of the tunnel loading as a robot moves through the environment. Attempting to report an artifact that was never loaded will fail. An artifact only has to be loaded once.

    The position of an artifact should be in the artifact origin frame. See the Artifacts section on this page.

  4. Martin Dlouhy reporter

    OK, thanks - that could be the case! I shorted the exploration cycle to just debug the reporting - OK, so if that particular tile is loaded than this code may work … OK, I will try it, thanks. 🙂

  5. Martin Dlouhy reporter

    p.s. BTW isn’t any of the unittest world with just an trivial artifact I could use? Even practice01 has it behind the heap of stones … (or in other words could you please create also description of unit test worlds? (to be honest I tried only the first simple 01)

  6. Arthur Schang

    Martin,

    You can report a score any-time after you either enter the tunnel or call the rosservice /subt/start . As Nate said, you have to at least be near enough to an artifact for the level it is in to load for it to be a candidate for successful reporting. If you do notice any artifacts that are reasonably detectable but you are unable to score a point on despite reporting the location properly, please report that in a separate issue. I would verify that you are appropriately localizing the artifact by referencing the practice world’s absolute artifact location as returning to the entrance and then reporting should result in a successful score if you are calling the communication API correctly and the object is localized within 5 meters.

    For reference, I use tunnel_practice_02 for unit testing although some of the small worlds released recently might be better candidates if they contain artifacts (I have not checked if they do).

    Good luck!

  7. Martin Dlouhy reporter

    Hi Arthur,

    thanks for hint 🙂tunnel_circuit_practice_02 is simpler so the simulation runs faster (but not as fast as for unit test world, when the control fails to catch). Hopefully soon I will confirm that this “issue” is solved. Thanks a lot both of you.

  8. Martin Dlouhy reporter

    Hi Arthur and Nate,
    just to be sure that I understand you correctly:

    1. if I send /subt/start then the robot can report say "Backpack at (0, 0, 0)" even without moving the robot and the report should return TRUE and the score should be still 0 and number of attempts should be reduced by 1, right?
    2. if robot is returning from the tunnels and tries to report artifact while approaching base station it will receive FALSE for both:
    • it is too far, so the signal is week and messages are not delivered
    • the position of artifact is in "undiscovered area"

    In the case 2 is it not possible which one is which, so it is not good idea to try to re-send positions of artifacts because it could be due to "prohibited position", correct?

  9. Martin Dlouhy reporter

    well, it looks like even the first case fails, i.e. it is not visible on the server:

    [Msg] Scoring has Started
    [Dbg] [LinearBatteryPlugin.cc:333] [Battery Plugin] Battery drain: 0 minutes passed.
    [Msg] Loaded level [95]
    [Dbg] [LinearBatteryPlugin.cc:333] [Battery Plugin] Battery drain: 1 minutes passed.
    

    and no score report

  10. Alfredo Bencomo

    Hi Martin,

    Nate or AJ should get back to you soon. In the meantime, isn’t the API documentation (i.e. the Artifact section) providing the information you are looking for? Otherwise, I wan’t to make sure it gets updated so it clarifies this for others as well. Thanks!

  11. Martin Dlouhy reporter

    Hi Alfredo,

    thanks for pointing me to the documentation - it looks almost 1:1 to me except:

    artifact.SerializeAsString()
    

    vs.

    std::string serializedData;
    if (!artifact.SerializeToString(&serializedData))
    

    but I would guess it should create the same string?

    It is also possible that I may not to see the result in the server output, and that I have to listen to confirmation via callback method?? Finally I should maybe focus on “composed docker” used on the cloud, but I would hope that this part would be the same, right?

  12. Zbyněk Winkler

    The API documentation page does not mention anything about the dependency on on-demand tile loading.

  13. Zbyněk Winkler

    It also does not say what we should see in the console. Which scoring attempts are echoed to the console? It seems Martin has found a case when scoring attempt is not echoed. Is this expected?

  14. Martin Dlouhy reporter

    Well, I am looking for some simple test case, which should surely work - I do not know if I successfully reported artifact (I guess not).

  15. Zbyněk Winkler

    It also says

    Teams should not try to report the same artifact multiple times. By doing that, you will negatively impact your score, as your reported pose will be incorrectly associated with another artifact.

    but repeated reports of the same thing are supported & ignored, right?

    It also does not say you can just leave the starting area to enable scoring - you don’t have to call “start”.

  16. Alfredo Bencomo

    Martin,

    I’ll go with the code in the doc. Yes, it will best if you try it again using docker-compose.

  17. Arthur Schang

    Martin,

    1. In short, yes. You can send successfully send an artifact report as soon as you call the rosservice /subt/start or have any vehicle enter the environment (thus automatically triggering /subt/start). Assuming you call the service from the starting area and immediately send some blatantly incorrect answer to the base station you’ll get back a confirmation that it was received, no score increment (assuming it’s blatantly incorrect), and the number of your attempts will be reduced by 1.
    2. I am currently in the process of experimenting with this, I had not tested with the new communication API so I’m currently working on that. I just tested with the old communication API and I received back “True” whether or not I was in range or the detection was correct. The two instances where you should not get a score increment despite reporting the correct artifact location are when you are too far from the base station to communicate and whenever the artifact is in an “undiscovered area” although the latter shouldn’t happen under normal behavior (explore, identify, report). I will test further and post back here with some clarification on this.

    In the API linked by Alfredo, it is stated:

    When a robot communicates with the base station, received artifact requests are acknowledged by sending back a message of type ArtifactScore. Keep in mind that the ACK can also be lost as any other message sent. Repeated artifact requests (a request is considered repeated when contains the same artifact type and position as a previously sent request) do not count as new requests for scoring purposes. In other words, a team will not lose an extra token while retrying to send a previously reported artifact.

    so I believe you actually can spam the same pose/classification repeatedly and it is only counted as 1 inquiry whether it is correct, incorrect, or has already been successfully reported.

    Whenever I attempt to send score reports (or any communication over the communication protocol) I see the following debug message on my SubT Ignition Console:

    [Dbg] [VisibilityRfModel.cc:80] Range: 120.633, Exp: 4.1, TX: 20, RX: -105.34 so the packet/registered name you are sending by may not be correctly configured. Also, this debug message is from a failed message send, I also get the same debug messages when messages are successfully sent.

  18. Arthur Schang

    Ok, I just looked into the new ACK interface on for reporting artifacts and my clarified answer for your # 2 is:

    If you report an artifact with a properly formed packet through the CommsClient you will always get back TRUE from the CommsClient on the call this->client->SendTo(serializedData, "base_station"). If you are using the new ACK system put in, you will be able to determine if the artifact report was valid with information in the ArtifactScore message. Due to the ACK being subject to the same communication restrictions and stochastic nature of communication, continually reporting the artifact report until you either get an ACK or enough radio silence to determine if the message is getting to the base station or not is a good idea while you can verify the accuracy of your report if/when you get that ACK. Whenever I sent a successful score report I received score_change: 1 in the ACK to indicate the artifact report was valid.

    In short, the boolean from this->client->SendTo(serializedData, "base_station") is insufficient in determining whether or not the message arrived at the base station or that the artifact was valid as it should always return TRUE if the message sent is valid. The FALSE results you are receiving from the CommsClient may be due to faulty usage or a malformed message. Additionally, monitoring ACK from the base station following a report is the correct method for V&V of the artifact report.

  19. Alfredo Bencomo

    Thanks Martin. I just updated the link in the API doc. Can we now resolve this issue or do you still need to run more tests?

  20. Martin Dlouhy reporter

    At the moment I have no idea what is wrong - I guess that the CommsClient() is not properly initialized?? Is there somewhere working example I could use? I do not see any messages in the console (I suppose that it is the same as were I see [Dbg] [LinearBatteryPlugin.cc:433] voltage: 12.6591 etc??)

  21. Arthur Schang

    Martin, I recommend you check out the subt_helloworld repository’s development branch. There is a package named “comm_example” that you might find beneficial. That example is functional.

  22. Alfredo Bencomo

    Martin,

    Another option since you mentioned that you were using Docker images, it’s to run the follow:

    Open terminal 1 and run cloudsim_sim:

    $ mkdir -p ~/subt_testbed && cd ~/subt_testbed
    $ wget https://bitbucket.org/osrf/subt/raw/tunnel_circuit/docker/run.bash
    $ chmod +x run.bash
    $ ./run.bash osrf/subt-virtual-testbed:cloudsim_sim_latest cloudsim_sim.ign robotName1:=X1 robotConfig1:=X1_SENSOR_CONFIG1
    

    Open terminal 2 and run cloudsim_bridge:

    $ cd ~/subt_testbed/
    $ ./run.bash osrf/subt-virtual-testbed:cloudsim_bridge_latest robotName1:=X1 robotConfig1:=X1_SENSOR_CONFIG1
    

    Open terminal 3 and run the subt_solution:

    $ cd ~/subt_testbed/
    $ ./run.bash osrf/subt-virtual-testbed:subt_solution_latest
    

    The solution running insideterminal 3 will take between 15-30 second to start (be patience). After that, you should see the start signal, the report message, and the score; as shown below:

    [ INFO] [1568762243.991832697, 37.596000000]: Sent start signal.
    [ INFO] [1568762256.265146241, 48.992000000]: Arrived at entrance!
    [ INFO] [1568762256.368889859, 49.092000000]: Message from [base_station] to [X1] on port [4100]:
     [report_id: 1
    artifact {
      type: 4
      pose {
        position {
          x: -8.1
          y: 37
          z: 0.004
        }
      }
    }
    submitted_datetime {
      sec: 95903
      nsec: 363782255
    }
    sim_time {
      sec: 49
    }
    run: 1
    report_status: "scored"
    score_change: 1
    

  23. Martin Dlouhy reporter

    Thank you both - I will start from scratch. The example works in the sense, that I see the report there:

    [Msg] Scoring has Started
    [Broker::DispatchMessages()]: Could not find endpoint X2:4100
    [Dbg] [LinearBatteryPlugin.cc:333] [Battery Plugin] Battery drain: 0 minutes passed.
    [Broker::DispatchMessages()]: Could not find endpoint X2:4100
    [Broker::DispatchMessages()]: Could not find endpoint X2:4100
    [Broker::DispatchMessages()]: Could not find endpoint X2:4100
    [Broker::DispatchMessages()]: Could not find endpoint X2:4100
    [Dbg] [VisibilityRfModel.cc:80] Range: 17.8337, Exp: 2.5, TX: 20, RX: -51.2811
    [Msg]   [Total]: 1
    [Msg] Total score: 1
    [Dbg] [VisibilityRfModel.cc:80] Range: 17.9024, Exp: 2.5, TX: 20, RX: -51.3228
    [Msg] Unloaded level [63]
    [Broker::DispatchMessages()]: Could not find endpoint X2:4100
    [Broker::DispatchMessages()]: Could not find endpoint X2:4100
    

    I was thinking that I was making some stupid mistake like roslaunch vs. ign launch?!

  24. Alfredo Bencomo

    I hope these examples can clarify things for you and be used as a reference. Feel free to reopen it if you still need more assistance. Cheers!

  25. Martin Dlouhy reporter

    I am checking the “hello world” example and this line is bit discouraging:

    bool report = this->client->SendTo(serializedData, "BaseStation"); //subt::kBaseStationName);
    

    ?!
    subt_hello_world\catkin_build_ws\src\comms_example\src\object_detection_node.cc:134

  26. Martin Dlouhy reporter

    Well, I am not sure, but it looks suspicious … in the WiKi is subt::kBaseStationName which is mapped to stringbase_station and it probably did not work for hello world, so the author put there directly the old BaseStation string? Or it depends in which setup you run it? (I did quick check with my old code and BaseStation string but it was not the source of the problem, or not the only one)

  27. Arthur Schang

    … check out the subt_helloworld repository’s development branch

    Just checked on the development branch and saw that is correctly set as “base_station”. Also reference: https://bitbucket.org/jgrogers/subt_hello_world/src/development/catkin_build_ws/src/comms_example/src/object_detection_node.cc line 138. So I suspect you pulled down and investigated the public branch by default. The public branch is currently outdated. The current base station string name is “base_station” as you noted. It is defined in subt_ign/include/subt_ign/CommonTypes.hh .

    Nonetheless, if you are using subt::kBaseStationName and have linked/included properly or are using “base_station” you should be good on this front. Are you still having issues or did you get this resolved? I notice the issue is now marked Resolved.

  28. Martin Dlouhy reporter

    Arthur, thanks a lot - yes I was looking at “public” (default) branch. Alfredo set this issue as resolved, but I cannot confirm that yet - if I will not be able to sort it out in 24hours I will reopen it, thanks.

  29. Arthur Schang

    Take your time Martin, I’ll keep checking back on this issue until you confirm you’ve got it resolved.

  30. Martin Dlouhy reporter
    • changed status to open

    Double-check on loading artifacts (Tunnel Practice 1 simulation with id a9bb9bab-6a7f-4312-9a42-541d303886b5) scoring 2 points from the entrance.

  31. Martin Dlouhy reporter

    The robot must get close enough to an artifact for it to be loaded before you can attempt to score with the artifact. You can see parts of the tunnel loading as a robot moves through the environment. Attempting to report an artifact that was never loaded will fail. An artifact only has to be loaded once.

    @Nate Koenig I would like to double check that the feature of loading artifacts is not used yet (to be honest, it would simplify the development at the moment, so I would prefer it the way how it works now)? I tried “Tunnel Practice 1 simulation with id a9bb9bab-6a7f-4312-9a42-541d303886b5” (robotika/seed2) which test reporting of two artifacts from the entrance (without exploration). As I said I would leave it for Tunnel Circuit as it is now as it adds extra complexity.

    Thanks

    Martin

  32. Arthur Schang

    You should be able to report a couple of artifacts from the start location because they’re in the same tile set as the entrance and thus are loaded when you start the environment. Are you trying to say the current issue is that you can report 2 artifacts from the entrance?

  33. Martin Dlouhy reporter

    🙂 yes. Note, that I am not complaining about this 😉 My impression was that “tile” is like the “crossing” or “turn” in the tunnel (I did not check the correspondence between integers and the world map) so that I expected that if robot did not visit/see the part say x=-8, y=30 or x=35 y=10 (I am just guessing the numbers now) then it should fail … ?? So if the set near staging area is larger (I did not locate any “deep artifact” yet for testing) then there is probably no problem at all. Thank you & feel free to close this issue again

  34. Joe Palk

    Sorry for reopening this, but I think my question is relevant to this thread. I’m currently trying to use the ACK system to confirm artifact reports, but it seems like the ACK’s take a long time (at least 20 seconds) to get back to the robot. (I’m testing artifact reports from the starting platform, with fake artifacts, before moving anywhere but after calling the “subt/start” service.)

    My specific question is this: when we instantiate the subt::CommsClient used to send and listen for messages from the base station, should the ‘_use_ignition’ parameter be set to true or false? I understand that Issue 144 and the code comments in subt_communication_client.h say that robots should always use “_use_ignition=false', but when I look at the BaseStationPlugin.cc it uses:

    bool BaseStationPlugin::Load(const tinyxml2::XMLElement *)
    {
      this->client.reset(new subt::CommsClient("base_station", true, true));
      this->client->Bind(&BaseStationPlugin::OnArtifact, this);
    

    In our comms manager, we setup the CommsClient with:

    CommsManager::CommsManager(const std::string &_address) : address(_address)
    {
        printf("Bind Address is [%s]\n",_address.c_str());
        this->client = std::make_unique<subt::CommsClient>(_address, false,false);
        this->client->Bind(&CommsManager::receive_receipt, this, _address, this->receipt_port);
    

    Could the difference between ‘_use_ignition’ values cause the ACK sent back by the base station to be delayed in getting to the robot? Thanks!

  35. Carlos Agüero

    Hi Jose, your assumption is correct. You should use _useIgnition = false. The ACKs shouldn’t be delayed, the only explanation that we find is that your simulation is running very slow, and then, 20 seconds in wall time might be expected. Could you tell us what is the real time factor when you report the artifact?

  36. Joe Palk

    Hi Carlos. I’m testing with an X1 and an X2 team, and the real time factor throughout my comms test fluctuates around 50-60%. I think I’ve established that the artifact report from my robots get to the base station almost instantaneously, the base station processes the report and sends back an acknowledgement just as quickly, but then it takes a long time for the report to get back to the robot. Maybe I’m doing something silly though - here are the relevant snippets from my code, where I’m trying to reproduce what’s done in ‘subt/subt_ign/test/score.cc’.

    Setup our comms manager:

    CommsManager::CommsManager(const std::string &_address) : address(_address)
    {
        printf("Bind Address is [%s]\n",_address.c_str());
        this->client = std::make_unique<subt::CommsClient>(_address, false,false);
        this->client->Bind(&CommsManager::receive_receipt, this, _address, this->receipt_port);
    }
    

    receive_receipt callback:

    void CommsManager::receive_receipt( const std::string &_srcAddress,
                                        const std::string &_dstAddress,
                                        const uint32_t _dstPort,
                                        const std::string &_data)
    {
        ROS_INFO_STREAM( "[" << this->address << "] CommsManager: Received RECEIPT "
                              << _srcAddress << "->" << _dstAddress <<  ":" << _dstPort);
    
        if(_srcAddress == "base_station"){
            receive_artifact_ACK(_srcAddress, _dstAddress, _dstPort, _data);
        }
        ...
    

    Which in turn calls the receive_artifact_ACK function when the message is from the base_station:

    void CommsManager::receive_artifact_ACK(const std::string &_srcAddress,
                                            const std::string &_dstAddress,
                                            const uint32_t _dstPort,
                                            const std::string &_data)
    {
        subt::msgs::ArtifactScore ack;
        if(!ack.ParseFromString(_data)){
            std::cerr<<"Error parsing artifact score response"<<std::endl;
        }
        else{
            auto realTime = std::chrono::steady_clock::now().time_since_epoch();
            auto seconds = std::chrono::duration_cast<std::chrono::seconds>(realTime);
            ignition::msgs::Time submission_time = ack.submitted_datetime();
            ROS_INFO_STREAM(address << " has received an ACK!");
            ROS_INFO_STREAM(address<< " ACK ID:"<<ack.report_id()<<", report status:"<< ack.report_status()<<", score change:"<<ack.score_change()<<
            "\n"<<"artifact = "<<ack.artifact().type()<<" at "<<ack.artifact().pose().position().x() << ", "<< ack.artifact().pose().position().y() <<", "<<ack.artifact().pose().position().z() << "\n submission time = "
            << submission_time.sec()<<
            "\n current time = "<<seconds.count());
            score_ACKs.push_back(ack);
        }
    }
    

    And finally the function where we send the artifact report to the base station and wait for the score_ACKs vector to change:

    bool CommsManager::send_to_base_station(const subt::msgs::Artifact &a)
    {
        using namespace std::chrono_literals;
        // Serialize the artifact.
        std::string serializedData;
        if (!a.SerializeToString(&serializedData))
        {
            std::cerr << "CommsClient::SendToBaseStation(): Error serializing message\n"
                      << a.DebugString() << std::endl;
            return false;
        }
    
        // Send data to the base station.
        ROS_INFO_STREAM("[" << this->address << "] TO BASE STATION (" << base_station_address << "): " << serializedData);
    
        int num_acks = score_ACKs.size();
    
        auto realTime = std::chrono::steady_clock::now().time_since_epoch();
        auto seconds = std::chrono::duration_cast<std::chrono::seconds>(realTime);
    
        ROS_INFO_STREAM("[" << this->address << "] # ACK = "<<score_ACKs.size()<< " at time "<< seconds.count());
        this->client->SendTo(serializedData, base_station_address, 4100);
    
        int attempt = 0;
        while(score_ACKs.size() == num_acks && attempt < 20){
            attempt++;
            std::this_thread::sleep_for(500ms);
            if(attempt % 10 == 0){
                realTime = std::chrono::steady_clock::now().time_since_epoch();
                seconds = std::chrono::duration_cast<std::chrono::seconds>(realTime);
                ROS_INFO_STREAM("[" << this->address << "] received # ACK = "<< score_ACKs.size() <<" on attempt = "<<attempt<<" at time "<<seconds.count());
            }
        }
    
        ROS_INFO_STREAM("[" << this->address << "] required # attempts to receive ACK = "<<attempt);
    
        if(score_ACKs.size() > num_acks){
            return true;
        }
        return false;
    }
    

    As an example of the output from the receive_artifact_ACK when we eventually receive an acknowledgement:

    ...
    [ INFO] [1569420641.165585155, 27.368000000]: X1 ACK ID:2, report status:scored, score change:0
    artifact = 2 at -3.5, 13.5, 10.0615
     submission time = 290489
     current time = 290509
    ...
    

    I’ve added some output lines to the subt comms code to figure out when the base station receives the report and sends back an acknowledgement, and the messages corresponding to the above receive_artifact_ACK output is:

    [Dbg] [VisibilityRfModel.cc:80] Range: 12.3695, Exp: 2.5, TX: 20, RX: -47.3088
    [Dbg] [GameLogicPlugin.cc:515] Score plugin received artifact at 290489
    [Msg]   [Total]: 0
    [Msg] Total score: 0
    [Dbg] [VisibilityRfModel.cc:80] Range: 13.0007, Exp: 2.5, TX: 20, RX: -47.8491
    [Dbg] [GameLogicPlugin.cc:515] Score plugin received artifact at 290489
    [Msg]   [Total]: 0
    [Msg] Total score: 0
    [Dbg] [BaseStationPlugin.cc:106] Sending ArtifactScore back to robot X1, REPORT ID = 2 at time 290489
    [Dbg] [subt_communication_client.cpp:301] Calling SendTo (Ignition) to X1 at time 290489
    

    So it looks like the robot sends a report at 290489, the base station receives and responds at 290489, but then our code doesn’t pick it up until 290509. Do you see anything on our end that might be responsible for the one-way delay? Thanks, and sorry for the very long post :-)

  37. Joe Palk

    Hi again! I’ve just noticed that the same delay issue with the base station seems to occur with the CommsClient::Neighbors function. I have not dug into it very much, but I’m mentioning it because it might be easier for you to debug than wading through my code above. I’ve modified our code to start a beacon:

    CommsManager::CommsManager(const std::string &_address) : address(_address)
    {
        printf("Bind Address is [%s]\n",_address.c_str());
        this->client = std::make_unique<subt::CommsClient>(_address, false,false);
        this->client->Bind(&CommsManager::receive_receipt, this, _address, this->receipt_port);
        this->client->StartBeaconInterval(ros::Duration(1.0));
    }
    

    And then I just constantly output the robot’s neighbors map:

    / Report this robot's neighbors
            subt::CommsClient::Neighbor_M neighbors = controller_->get_neighbors();
            subt::CommsClient::Neighbor_M::iterator it = neighbors.begin();
            while(it != neighbors.end()){
                ROS_INFO_STREAM(this->controller_->get_name() << " has neighbor "<<it->first<<" at time "<<it->second.first << " with signal strength "<<it->second.second<<" dBm");
                it++;
            }
    

    What I see is that the two robots immediately register each other as neighbors, but it takes a much longer time for them to ‘see’ the base_station as a neighbor. Initially the neighbors for X1 are:

    [ INFO] [1569425405.733792013, 27.100000000]: X1 has neighbor X1 at time 27.052 with signal strength inf dBm
    [ INFO] [1569425405.733835913, 27.100000000]: X1 has neighbor X2 at time 27.048 with signal strength -29.9188 dBm
    ...
    

    And about 20-30 seconds later it picks up the base station:

    [ INFO] [1569425446.994545179, 51.200000000]: X1 has neighbor X1 at time 27.348 with signal strength inf dBm
    [ INFO] [1569425446.994567261, 51.200000000]: X1 has neighbor X2 at time 27.184 with signal strength -29.8888 dBm
    [ INFO] [1569425446.994584942, 51.200000000]: X1 has neighbor base_station at time 51.2 with signal strength -53.1534 dBm
    ...
    

  38. Log in to comment