Evaluate integration of waypoints and lanes to the graph

[osrf-migration]

Original report (archived issue) by Agustin Alba Chicar (Bitbucket: agalbachicar).

---

These are two use cases where we will benefit from having the Waypoints and Lanes inside the Graph structure.

1. When building the junctions, we iterate through the lanes and the perimeters to get the exits and entries. Then, we have to get the reference to the waypoints to get its location.
2. When sending information to the Box plugin, and getting the waypoints of a complete lane and its exits.

In both cases, we need to call some functions like the following:

#!c++

manifold::rndf::Waypoint DynamicRender::GetWaypointByUniqueId(
  const manifold::rndf::UniqueId &waypointId) {
  try {
    return GetWaypointInSegmentsByUniqueId(waypointId);
  }
  catch(gazebo::common::Exception &e) {
  }
  return GetWaypointInZonesByUniqueId(waypointId);
}

manifold::rndf::Waypoint DynamicRender::GetWaypointInSegmentsByUniqueId(
  const manifold::rndf::UniqueId &waypointId) {
  std::vector<manifold::rndf::Segment> &segments =
  rndfInfo->Segments();
  for (uint i = 0; i < segments.size(); i++) {
    manifold::rndf::Segment &segment = segments[i];
    if (segment.Id() != waypointId.X())
      continue;
    std::vector<manifold::rndf::Lane> &lanes =
      segment.Lanes();
    for (uint j = 0; j < lanes.size(); j++) {
      manifold::rndf::Lane &lane = lanes[j];
      if (lane.Id() != waypointId.Y())
        continue;
      std::vector<manifold::rndf::Waypoint> waypoints =
        lane.Waypoints();
      for (uint k = 0; k < waypoints.size(); k++) {
        manifold::rndf::Waypoint &waypoint = waypoints[k];
        if (waypoint.Id() == waypointId.Z())
          return waypoint;
      }
      gzthrow("Error. Waypoint " +
        createWaypointName(waypointId.X(),
          waypointId.Y(),
          waypointId.Z()) +
        " has not been found in segments.");
    }
    gzthrow("Error. Waypoint " +
      createWaypointName(waypointId.X(),
        waypointId.Y(),
        waypointId.Z()) +
      " has not been found in segments.");
  }
  gzthrow("Error. Waypoint " +
    createWaypointName(waypointId.X(),
      waypointId.Y(),
      waypointId.Z()) +
    " has not been found in segments.");
}

manifold::rndf::Waypoint DynamicRender::GetWaypointInZonesByUniqueId(
  const manifold::rndf::UniqueId &waypointId) {
  if (waypointId.Y() != 0) {
    gzthrow("Error. Waypoint " +
      createWaypointName(waypointId.X(),
        waypointId.Y(),
        waypointId.Z()) +
      " has not been found in zones.");
  }
  std::vector<manifold::rndf::Zone> &zones =
  rndfInfo->Zones();
  for (uint i = 0; i < zones.size(); i++) {
    manifold::rndf::Zone &zone = zones[i];
    if (zone.Id() != waypointId.X())
      continue;
    manifold::rndf::Perimeter &perimeter =
      zone.Perimeter();
    std::vector<manifold::rndf::Waypoint> &waypoints =
      perimeter.Points();
    for (uint j = 0; j < waypoints.size(); j++) {
      manifold::rndf::Waypoint waypoint =
        waypoints[j];
      if (waypoint.Id() == waypointId.Z())
        return waypoint;
    }
    gzthrow("Error. Waypoint " +
      createWaypointName(waypointId.X(),
        waypointId.Y(),
        waypointId.Z()) +
      " has not been found in zones.");
  }
  gzthrow("Error. Waypoint " +
    createWaypointName(waypointId.X(),
      waypointId.Y(),
      waypointId.Z()) +
    " has not been found in zones.");
}

manifold::rndf::Lane* DynamicRender::GetLaneByUniqueId(
  const manifold::rndf::UniqueId &waypointId) {
  std::vector<manifold::rndf::Segment> &segments =
  rndfInfo->Segments();
  for (uint i = 0; i < segments.size(); i++) {
    manifold::rndf::Segment &segment = segments[i];
    if (segment.Id() != waypointId.X())
      continue;
    std::vector<manifold::rndf::Lane> &lanes =
      segment.Lanes();
    for (uint j = 0; j < lanes.size(); j++) {
      manifold::rndf::Lane &lane = lanes[j];
      if (lane.Id() != waypointId.Y())
        continue;
      std::vector<manifold::rndf::Waypoint> waypoints =
        lane.Waypoints();
      for (uint k = 0; k < waypoints.size(); k++) {
        manifold::rndf::Waypoint &waypoint = waypoints[k];
        if (waypoint.Id() == waypointId.Z())
          return &lane;
      }
      return NULL;
    }
    return NULL;
  }
  return NULL;
}

To see these functions in context, please refer to the following link. As you can see we need to iterate through the whole structure of segments and/or zones to reach a lane.

I mention @andres_fortier so he is notified with the discussion.

[osrf-migration]

Original comment by Agustin Alba Chicar (Bitbucket: agalbachicar).

---

• Edited issue description

[osrf-migration]

Original comment by Agustin Alba Chicar (Bitbucket: agalbachicar).

---

• Edited issue description

[osrf-migration]

Original comment by Carlos Agüero (Bitbucket: caguero, GitHub: caguero).

---

@agalbachicar , @andres_fortier, I believe the requested functionality has been implemented in f2112cf029186da7752167ca08944927082a62a5. Please, take a look when you have a chance.

[osrf-migration]

Original comment by Agustin Alba Chicar (Bitbucket: agalbachicar).

---

@caguero I think that it will be pretty handy to have a pointer to the Waypoint in the manifold::rndf::RoadNode class. For example, if I want to search for a waypoint I will do something like:

#!c++

std::string strId = "1.2.3";
manifold::RNDFNode *node = rndfInfo->Info(strId);
if (node == nullptr) {
  // The waypoint does not exist.
}
else {
  // I can pick the waypoint pointer
  manifold::rndf::Waypoint *waypoint = node->Waypoint();
}

I think this way, we join all the API points. Because from the graph, you get your desired vertex. And then you can get the reference to the object given the name of the vertex. For future discussion, I think that a common interface for grabbing the vertex metadata will be nice.

[osrf-migration]

Original comment by Carlos Agüero (Bitbucket: caguero, GitHub: caguero).

---

@agalbachicar , I added the pointer to the waypoint in the RNDFNode class (d596d21ed3800b2276769a646c3a8f9e00bfd131). Thanks for the suggestion.