def make_edge(from_waypoint_id, to_waypoint_id, from_tform_to, edge_environment=None):
"""Create an edge between two waypoint ids.
Args:
from_waypoint_id: A waypoint string id for the from waypoint.
to_waypoint_id: A waypoint string id for the to waypoint.
from_tform_to: An SE3Pose representing the transform of from_waypoint to to_waypoint.
edge_environment: Any edge environment to be associated with the created edge.
Returns:
The API Edge protobuf message.
"""
edge_id = map_pb2.Edge.Id(from_waypoint=from_waypoint_id, to_waypoint=to_waypoint_id)
edge = map_pb2.Edge(id=edge_id, from_tform_to=from_tform_to)
if edge_environment is not None:
edge.annotations.CopyFrom(edge_environment)
return edge
def _create_new_edge(self, *args):
"""Create new edge between existing waypoints in map."""
if len(args[0]) != 2:
print(
"ERROR: Specify the two waypoints to connect (short code or annotation)."
)
return
if self._current_graph is None:
self._current_graph = self._graph_nav_client.download_graph()
from_id = graph_nav_util.find_unique_waypoint_id(
args[0][0], self._current_graph,
self._current_annotation_name_to_wp_id)
to_id = graph_nav_util.find_unique_waypoint_id(
args[0][1], self._current_graph,
self._current_annotation_name_to_wp_id)
print("Creating edge from {} to {}.".format(from_id, to_id))
from_wp = self._get_waypoint(from_id)
if from_wp is None:
return
to_wp = self._get_waypoint(to_id)
if to_wp is None:
return
# Get edge transform based on kinematic odometry
edge_transform = self._get_transform(from_wp, to_wp)
# Define new edge
new_edge = map_pb2.Edge()
new_edge.id.from_waypoint = from_id
new_edge.id.to_waypoint = to_id
new_edge.from_tform_to.CopyFrom(edge_transform)
print("edge transform =", new_edge.from_tform_to)
# Send request to add edge to map
self._recording_client.create_edge(edge=new_edge)