Performing Range Bearing using a CPP client

To perform range bearing you will need to understand Ignition transport and the acoustic comms layer. The range-bearing protocol uses the underlying Comms architecture to simulate a very basic instance of two way ranging. For an example of how to use range bearing see the following unit test

External API for invoking the range bearing plugin.

The RangeBearingPlugin will listen on the /{namespace}/range_bearing/requests for incoming requests using the lrauv_ignition_plugins::msgs::LRAUVRangeBearingRequest message and will respond using the lrauv_ignition_plugins::msgs::LRAUVRangeBearingResponse message on the /{namespace}/range_bearing/response topic.

The RangeBearingResponse has the following format:

message LRAUVRangeBearingResponse
{
  /// \brief Request ID. Each request ID must be unique.
  uint32 req_id = 1;

  /// \brief Bearing in terms of (r, theta, phi). This is based on ground truth
  /// and not calculated using time-of-flight
  ignition.msgs.Vector3d bearing = 2;

  /// \brief Time-Of-Flight reading. This is good for studying errors arising
  /// due to movement. But at high dt this may be prone to error.
  double range = 3;
}

An example client

This client is the same as the one found in the unit test.

class RangeBearingClient
{

public: RangeBearingClient(std::string namespace) :
  request_counter(2)
{
  this->pub = this->node.Advertise<msgs::LRAUVRangeBearingRequest>(
    "/"+namespace+"/range_bearing/requests");
  this->node.Subscribe("/"+namespace+"/range_bearing/responses",
    &RangeBearingClient::onRecvResponse,
    this
  );
}

/// \brief Use this method to range the vehicle with address address
/// \param[\in] address - The address to range with. 
public: msgs::LRAUVRangeBearingResponse RequestRange(uint32_t address)
{
  // Send a range bearing request targetting <address>
  msgs::LRAUVRangeBearingRequest req;
  req.set_to(address);

  // We use a request counter to help synchronize messages with responses
  req.set_req_id(this->request_counter);

  // Reset the condition to be waited upon.
  this->received = false;
  this->pub.Publish(req);

  // wait for response
  std::unique_lock<std::mutex> lk(this->mtx);
  cv.wait(lk, [this]{return this->received;});

  // Increment request counter.
  this->request_counter++;

  return this->lastResponse;
}

/// \brief this callback is called when a response comes in.
public: void onRecvResponse(const msgs::LRAUVRangeBearingResponse& resp)
{
  igndbg << "Received message\n";
  if(resp.req_id() == this->request_counter)
  {
    {
      std::lock_guard<std::mutex> lk(this->mtx);
      this->lastResponse = resp;
      this->received = true;
    }
    cv.notify_all();
  }
}

private: ignition::transport::Node node;

private: ignition::transport::Node::Publisher pub;

private: uint32_t request_counter;

private: std::mutex mtx;

private: std::condition_variable cv;

private: msgs::LRAUVRangeBearingResponse lastResponse;

private: bool received;
};