problem when transmitting data

[Aimiliospet]

Hello everyone, I'm trying to test for now a single transmission of some sensor data using the mc_udp class and I'm not able to retrieve the data with the client. I'm following the example of the dummy codes in the repository but something does not work for me. I think the problem lies with the mc_udp::RobotControl & control reference and this does not retrieve the sent data from the server. Is this not the right way to acquire the sent data? Here for example at the end sensors.id and sensors.encoders dont contain the right values although i think the connection is established. (I get the messages: Start streaming data to client and the program goes in the if statement with the client.recv() function) code for getting the data in the client instance:

    int port = 4444;
    std::string host = "localhost";
    std::shared_ptr<mc_udp::Client> clientptr;
    mc_udp::RobotControl controls;
    clientptr.reset(new mc_udp::Client(host,port));
    auto & controls = clientptr->control().messages["wb_data"];
    uint64_t prev_id = 0;
    bool init = false;

if(clientptr->recv())
    {
      if(!init)
      {
        init = true;
        clientptr->init();
      } 
      const auto & sensors = clientptr->sensors().messages.at("wb_data");
      std::cout << sensors.id << " -> this is the sensors id" << std::endl;
      q_.assign(sensors.encoders.begin(), sensors.encoders.end()); //try to save sent values in q_ vector
      std::cout << "sensors encoders size = " << sensors.encoders.size() << std::endl;

code for sending the data from the server instance:

    int port = 4444;
    std::shared_ptr<mc_udp::Server> serverptr;
    mc_udp::RobotSensors sensors;
    serverptr.reset(new mc_udp::Server(port));
    auto & sensors = serverptr->sensors().messages["wb_data"];
    sensors.id = 0;
    some_update_function(const ros::Time& time, const ros::Duration& period){
      ros::spinOnce();

      sensors.encoders.assign(q_.begin(), q_.end());
      sensors.encoders.assign(qp_.begin(), qp_.end());
      sensors.encoders.assign(qeff_.begin(), qeff_.end());
      serverptr->send();
      sensors.id += 1 ;}

By the way as you see this is not the whole code, just the relevant part for using mc_udp and i think the problem lies in the client intsance in the sensors reference. Do i miss something important here or isn't this the way to get the transmitted data? Right now the sensors.id is always 0 and the sensors.encoders have size 0 so i cant have access to sent data with this. Thanks in advace!

[gergondet]

Hi @Aimiliospet

I think the way you get the references to the sensors is ok. Maybe you are missing a recv() on the server side? This is required to detect new clients.

Here are two simpler examples than the dummy server/clients:

First the server:

#include <mc_udp/server/Server.h>

#include <chrono>
#include <memory>
#include <thread>

int main()
{
  auto server = std::make_shared<mc_udp::Server>(4444);
  for(size_t i = 0; i < 1000; ++i)
  {
    auto & data = server->sensors().messages["ROBOT"];
    data.encoders.resize(6);
    for(size_t i = 0; i < data.encoders.size(); ++i)
    {
      data.encoders[i] = data.id * 1.1 * (i + 1);
    }
    server->send();
    if(server->recv())
    {
      // do something with control data
    }
    data.id += 1;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  return 0;
}

And the client, which also demonstrates how to write so it can reconnect or connect even if it is started after the server:

#include <mc_udp/client/Client.h>

#include <eigen3/Eigen/Core>

#include <chrono>
#include <iostream>
#include <memory>

int main()
{
  auto client = std::make_shared<mc_udp::Client>("localhost", 4444);
  bool init = false;
  size_t n_recv = 0;
  auto last_recv = std::chrono::high_resolution_clock::now();
  while(n_recv < 100000)
  {
    if(client->recv())
    {
      if(!init)
      {
        init = true;
        client->init();
      }
      last_recv = std::chrono::high_resolution_clock::now();
      auto & data = client->sensors().messages.at("ROBOT");
      Eigen::Map<const Eigen::VectorXd> encoders(data.encoders.data(), data.encoders.size());
      std::cout << "encoders: " << encoders.transpose() << "\n";
      n_recv++;
      auto & control = client->control();
      control.messages["ROBOT"].id = data.id;
      client->send();
    }
    else
    {
      // Attempt to re-connect if we didn't receive anything for 1 second
      auto since_last_recv = std::chrono::high_resolution_clock::now() - last_recv;
      if(since_last_recv > std::chrono::seconds(1))
      {
        init = false;
        client->sendHello();
        last_recv = std::chrono::high_resolution_clock::now();
      }
    }
  }
  return 0;
}