diff --git a/Dockerfile b/Dockerfile
index ca1e39ec1..afe228965 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -120,7 +120,8 @@ RUN pip3 install \
eventlet \
python-socketio \
python-socketio[client] \
- websocket-client
+ websocket-client \
+ asrl-pylgmath
#added by sherry
@@ -200,5 +201,36 @@ COPY entrypoint.sh /usr/local/bin/entrypoint.sh
RUN chmod +x /usr/local/bin/entrypoint.sh
ENTRYPOINT ["/usr/local/bin/entrypoint.sh"]
-## Switch to specified user
+# Clone libtorrent RC_2_0
+WORKDIR $HOMEDIR
+RUN git clone --recursive https://github.com/arvidn/libtorrent.git -b RC_2_0
+
+WORKDIR $HOMEDIR/libtorrent/build
+RUN cmake -DCMAKE_BUILD_TYPE=Release \
+ -DTORRENT_USE_ASSERTS=ON \
+ -Dpython-bindings=ON \
+ -Dencryption=ON \
+ -Ddht=ON \
+ -Dcrypto=openssl \
+ -Dextensions=ON \
+ -DPYTHON_EXECUTABLE=/usr/bin/python3.10 \
+ -DCMAKE_CXX_FLAGS="-DTORRENT_USE_OPENSSL=1" \
+ .. && \
+ make -j$(nproc)
+
+RUN nm -D bindings/python/libtorrent*.so | grep -E "dht_put_item|ed25519"
+RUN cp $HOMEDIR/libtorrent/build/bindings/python/libtorrent*.so /usr/local/lib/python3.10/dist-packages/libtorrent.so
+
+# Install Zenoh router (zenohd)
+RUN echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo /" | \
+ tee /etc/apt/sources.list.d/zenoh.list > /dev/null && \
+ apt-get update && \
+ printf '#!/bin/sh\nexit 0' > /usr/bin/systemctl && \
+ chmod +x /usr/bin/systemctl && \
+ apt-get install -y zenohd
+
+RUN pip3 install pynacl eclipse-zenoh msgpack zenoh-cli pprintpp pandas
+RUN pip3 install --upgrade matplotlib
+
+WORKDIR $HOME
USER ${USERID}:${GROUPID}
diff --git a/Dockerfile.CPU b/Dockerfile.CPU
index 3409b1eff..10fa13a28 100644
--- a/Dockerfile.CPU
+++ b/Dockerfile.CPU
@@ -158,6 +158,38 @@ COPY entrypoint.sh /usr/local/bin/entrypoint.sh
RUN chmod +x /usr/local/bin/entrypoint.sh
ENTRYPOINT ["/usr/local/bin/entrypoint.sh"]
+
+# Clone libtorrent RC_2_0
+WORKDIR $HOMEDIR
+RUN git clone --recursive https://github.com/arvidn/libtorrent.git -b RC_2_0
+
+WORKDIR $HOMEDIR/libtorrent/build
+RUN cmake -DCMAKE_BUILD_TYPE=Release \
+ -DTORRENT_USE_ASSERTS=ON \
+ -Dpython-bindings=ON \
+ -Dencryption=ON \
+ -Ddht=ON \
+ -Dcrypto=openssl \
+ -Dextensions=ON \
+ -DPYTHON_EXECUTABLE=/usr/bin/python3.10 \
+ -DCMAKE_CXX_FLAGS="-DTORRENT_USE_OPENSSL=1" \
+ .. && \
+ make -j$(nproc)
+
+RUN nm -D bindings/python/libtorrent*.so | grep -E "dht_put_item|ed25519"
+RUN cp $HOMEDIR/libtorrent/build/bindings/python/libtorrent*.so /usr/local/lib/python3.10/dist-packages/libtorrent.so
+
+# Install Zenoh router (zenohd)
+RUN echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo /" | \
+ tee /etc/apt/sources.list.d/zenoh.list > /dev/null && \
+ apt-get update && \
+ printf '#!/bin/sh\nexit 0' > /usr/bin/systemctl && \
+ chmod +x /usr/bin/systemctl && \
+ apt-get install -y zenohd
+
+RUN pip3 install pynacl eclipse-zenoh msgpack zenoh-cli pprintpp pandas
+RUN pip3 install --upgrade matplotlib
+
+WORKDIR $HOME
## Switch to specified user
-USER ${USERID}:${GROUPID}
-CMD ["bash"]
\ No newline at end of file
+USER ${USERID}:${GROUPID}
\ No newline at end of file
diff --git a/Dockerfile.jetson b/Dockerfile.jetson
index 01bd405ed..d339730fe 100644
--- a/Dockerfile.jetson
+++ b/Dockerfile.jetson
@@ -156,5 +156,39 @@ COPY entrypoint.sh /usr/local/bin/entrypoint.sh
RUN chmod +x /usr/local/bin/entrypoint.sh
ENTRYPOINT ["/usr/local/bin/entrypoint.sh"]
+
+# Clone libtorrent RC_2_0
+WORKDIR $HOMEDIR
+RUN git clone --recursive https://github.com/arvidn/libtorrent.git -b RC_2_0
+
+WORKDIR $HOMEDIR/libtorrent/build
+RUN cmake -DCMAKE_BUILD_TYPE=Release \
+ -DTORRENT_USE_ASSERTS=ON \
+ -Dpython-bindings=ON \
+ -Dencryption=ON \
+ -Ddht=ON \
+ -Dcrypto=openssl \
+ -Dextensions=ON \
+ -DPYTHON_EXECUTABLE=/usr/bin/python3.10 \
+ -DCMAKE_CXX_FLAGS="-DTORRENT_USE_OPENSSL=1" \
+ .. && \
+ make -j$(nproc)
+
+RUN nm -D bindings/python/libtorrent*.so | grep -E "dht_put_item|ed25519"
+RUN cp $HOMEDIR/libtorrent/build/bindings/python/libtorrent*.so /usr/local/lib/python3.10/dist-packages/libtorrent.so
+
+# Install Zenoh router (zenohd)
+RUN echo "deb [trusted=yes] https://download.eclipse.org/zenoh/debian-repo /" | \
+ tee /etc/apt/sources.list.d/zenoh.list > /dev/null && \
+ apt-get update && \
+ printf '#!/bin/sh\nexit 0' > /usr/bin/systemctl && \
+ chmod +x /usr/bin/systemctl && \
+ apt-get install -y zenohd
+
+RUN pip3 install pynacl eclipse-zenoh msgpack zenoh-cli pprintpp pandas
+RUN pip3 install --upgrade matplotlib
+
+WORKDIR $HOME
+
## Switch to specified user
USER ${USERID}:${GROUPID}
diff --git a/config/hdl64_grizzly_default.yaml b/config/hdl64_grizzly_default.yaml
index 87fd65d79..4073a99ab 100644
--- a/config/hdl64_grizzly_default.yaml
+++ b/config/hdl64_grizzly_default.yaml
@@ -332,7 +332,7 @@
min_vel: 0.5
mpc:
# Controller Params
- extrapolate_robot_pose: false
+ extrapolate_robot_pose: true
mpc_verbosity: false
forward_vel: 1.0
max_lin_vel: 1.5
diff --git a/config/ouster_hunter_default.yaml b/config/ouster_hunter_default.yaml
index 7139627b3..42992a47a 100644
--- a/config/ouster_hunter_default.yaml
+++ b/config/ouster_hunter_default.yaml
@@ -5,16 +5,16 @@
log_enabled:
# navigator
#- navigation
- #- navigation.graph_map_server
+ - navigation.graph_map_server
#- navigation.command
# tactic
- tactic
- #- tactic.pipeline
+ - tactic.pipeline
#- tactic.eop
#- tactic.module
- #- tactic.module.live_mem_manager
- #- tactic.module.graph_mem_manager
+ - tactic.module.live_mem_manager
+ - tactic.module.graph_mem_manager
# path planner
#- path_planning
@@ -34,14 +34,14 @@
#- ouster
# mission planner
- #- mission.server
- #- mission.state_machine
+ - mission.server
+ - mission.state_machine
# pose graph
- #- pose_graph
+ - pose_graph
# lidar pipeline
- #- lidar.pipeline
+ - lidar.pipeline
#- lidar.preprocessing
#- lidar.ouster_converter
#- lidar.odometry_icp
diff --git a/main/src/torrent_planner/package.xml b/main/src/torrent_planner/package.xml
new file mode 100644
index 000000000..1889ca666
--- /dev/null
+++ b/main/src/torrent_planner/package.xml
@@ -0,0 +1,20 @@
+
+
+
+ torrent_planner
+ 0.0.0
+ TODO: Package description
+ asrl
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+ vtr_navigation_msgs
+
+
+ ament_python
+
+
diff --git a/main/src/torrent_planner/resource/torrent_planner b/main/src/torrent_planner/resource/torrent_planner
new file mode 100644
index 000000000..e69de29bb
diff --git a/main/src/torrent_planner/setup.cfg b/main/src/torrent_planner/setup.cfg
new file mode 100644
index 000000000..ff60f9ec7
--- /dev/null
+++ b/main/src/torrent_planner/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/torrent_planner
+[install]
+install_scripts=$base/lib/torrent_planner
diff --git a/main/src/torrent_planner/setup.py b/main/src/torrent_planner/setup.py
new file mode 100644
index 000000000..b1e510d7b
--- /dev/null
+++ b/main/src/torrent_planner/setup.py
@@ -0,0 +1,30 @@
+from setuptools import find_packages, setup
+
+package_name = 'torrent_planner'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='asrl',
+ maintainer_email='asrl@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ extras_require={
+ 'test': [
+ 'pytest',
+ ],
+ },
+ entry_points={
+ 'console_scripts': [
+ 'torrent_planner = torrent_planner.torrent_planner:main'
+ ],
+ },
+)
diff --git a/main/src/torrent_planner/test/test_copyright.py b/main/src/torrent_planner/test/test_copyright.py
new file mode 100644
index 000000000..97a39196e
--- /dev/null
+++ b/main/src/torrent_planner/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/main/src/torrent_planner/test/test_flake8.py b/main/src/torrent_planner/test/test_flake8.py
new file mode 100644
index 000000000..27ee1078f
--- /dev/null
+++ b/main/src/torrent_planner/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/main/src/torrent_planner/test/test_pep257.py b/main/src/torrent_planner/test/test_pep257.py
new file mode 100644
index 000000000..b234a3840
--- /dev/null
+++ b/main/src/torrent_planner/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'
diff --git a/main/src/torrent_planner/torrent_planner/__init__.py b/main/src/torrent_planner/torrent_planner/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/main/src/torrent_planner/torrent_planner/torrent_planner.py b/main/src/torrent_planner/torrent_planner/torrent_planner.py
new file mode 100644
index 000000000..695d22cde
--- /dev/null
+++ b/main/src/torrent_planner/torrent_planner/torrent_planner.py
@@ -0,0 +1,122 @@
+import rclpy
+from rclpy.node import Node
+
+import numpy as np
+import uuid
+import os
+
+from vtr_navigation_msgs.msg import GraphUpdate, GraphState, GraphVertex
+from vtr_navigation_msgs.msg import MissionCommand, GoalHandle
+
+import pdb
+
+class TorrentPlanner(Node):
+ def __init__(self):
+ super().__init__('torrent_planner')
+ self.robot_name = os.getenv("ROBOT_NAME")
+ print(f"ROBOT_NAME: {self.robot_name}")
+ self.last_vtx = 0
+ self.following_gap = 10 # vertices
+ self.first_pub = False # first pub comes from graphUpdate, subsequent from graphState
+
+ self.command_pub = self.create_publisher(
+ MissionCommand,
+ f'/{self.robot_name}/vtr/mission_command',
+ 10
+ )
+
+ self.graph_state_sub = self.create_subscription(
+ GraphState,
+ f'{self.robot_name}/vtr/graph_state',
+ self.state_callback,
+ 10)
+ self.graph_update_sub = self.create_subscription(
+ GraphUpdate,
+ f'{self.robot_name}/vtr/graph_update',
+ self.update_callback,
+ 10)
+
+ def pack_and_publish_msg(self, command_vtx):
+ # wrap into a ADD_GOAL mission command
+ goal_handle = GoalHandle(
+ id=list(uuid.uuid4().bytes), # UUID
+ type=2, # repeat
+ pause_before=0,
+ pause_after=0,
+ waypoints=[command_vtx],
+ controller_name=''
+ )
+
+ add_goal = MissionCommand(
+ type=1, # add goal
+ pause=False,
+ goal_handle=goal_handle,
+ vertex=command_vtx,
+ # window=? don't set window
+ )
+ self.command_pub.publish(add_goal)
+
+ # wrap into a BEGIN_GOAL mission command
+ goal_handle = GoalHandle(
+ id=list(uuid.uuid4().bytes), # UUID
+ type=0, # repeat
+ pause_before=0,
+ pause_after=0,
+ # waypoints=[self.last_vtx], # dont set waypoints
+ controller_name=''
+ )
+
+ begin_goal = MissionCommand(
+ type=3, # add goal
+ pause=False,
+ goal_handle=goal_handle,
+ vertex=0,
+ # window=? don't set window
+ )
+ self.command_pub.publish(begin_goal)
+
+ def state_callback(self, graph_state):
+ # get largest vid in the posegraph
+ for vtx in graph_state.vertices:
+ if vtx.id > self.last_vtx:
+ self.last_vtx = vtx.id
+
+ rid = (self.last_vtx >> 60) & 0xF
+ major_id = (self.last_vtx >> 16) & 0xFFFFFFFFFFF
+ minor_id = self.last_vtx & 0xFFFF
+ print(f"vtx: {self.last_vtx}: robot_id {rid} | major_id {major_id} | minor_id {minor_id}")
+
+ print("pub via graph state")
+ if minor_id > self.following_gap:
+ command_vtx = self.last_vtx - self.following_gap
+ else:
+ command_vtx = self.last_vtx
+ self.pack_and_publish_msg(command_vtx)
+
+ def update_callback(self, graph_update):
+ if self.first_pub == True:
+ return
+
+ if graph_update.vertex_from.id - self.following_gap > self.last_vtx:
+ print("pub via graph update")
+ self.last_vtx = graph_update.vertex_from.id
+ rid = (self.last_vtx >> 60) & 0xF
+ major_id = (self.last_vtx >> 16) & 0xFFFFFFFFFFF
+ minor_id = self.last_vtx & 0xFFFF
+
+ if minor_id > self.following_gap:
+ command_vtx = self.last_vtx - self.following_gap
+ else:
+ command_vtx = self.last_vtx
+ self.first_pub = True
+ self.pack_and_publish_msg(command_vtx)
+
+def main(args=None):
+ rclpy.init(args=args)
+ torrent_planner = TorrentPlanner()
+ rclpy.spin(torrent_planner)
+ torrent_planner.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/main/src/vtr_gui/vtr_gui/socket_client.py b/main/src/vtr_gui/vtr_gui/socket_client.py
index cea56e196..2ecf4eb7d 100755
--- a/main/src/vtr_gui/vtr_gui/socket_client.py
+++ b/main/src/vtr_gui/vtr_gui/socket_client.py
@@ -34,9 +34,10 @@
def graph_state_from_ros(ros_graph_state):
return {
+ 'root_vid': str(ros_graph_state.root_vid),
'vertices': [{
- 'id': v.id,
- 'neighbors': [n for n in v.neighbors],
+ 'id': str(v.id),
+ 'neighbors': [str(n) for n in v.neighbors],
'lng': v.lng,
'lat': v.lat,
'theta': v.theta,
@@ -44,11 +45,11 @@ def graph_state_from_ros(ros_graph_state):
'name': v.name
} for v in ros_graph_state.vertices],
'fixed_routes': [{
- 'ids': [id for id in r.ids],
+ 'ids': [str(id) for id in r.ids],
'type': r.type
} for r in ros_graph_state.fixed_routes],
'active_routes': [{
- 'ids': [id for id in r.ids],
+ 'ids': [str(id) for id in r.ids],
'type': r.type
} for r in ros_graph_state.active_routes],
}
@@ -59,8 +60,8 @@ def graph_update_from_ros(ros_graph_update):
vt = ros_graph_update.vertex_to
return {
'vertex_from': {
- 'id': vf.id,
- 'neighbors': [n for n in vf.neighbors],
+ 'id': str(vf.id),
+ 'neighbors': [str(n) for n in vf.neighbors],
'lng': vf.lng,
'lat': vf.lat,
'theta': vf.theta,
@@ -68,8 +69,8 @@ def graph_update_from_ros(ros_graph_update):
'name': vf.name
},
'vertex_to': {
- 'id': vt.id,
- 'neighbors': [n for n in vt.neighbors],
+ 'id': str(vt.id),
+ 'neighbors': [str(n) for n in vt.neighbors],
'lng': vt.lng,
'lat': vt.lat,
'theta': vt.theta,
@@ -102,7 +103,7 @@ def map_info_from_ros(ros_map_info):
}
def following_route_from_ros(ros_following_route):
- return {'ids': [id for id in ros_following_route.ids]}
+ return {'ids': [str(id) for id in ros_following_route.ids]}
def goal_handle_from_ros(ros_goal_handle):
@@ -124,8 +125,8 @@ def goal_handle_from_ros(ros_goal_handle):
goal_handle["pause_before"] = ros_goal_handle.pause_before / 1000.0
# pause after
goal_handle["pause_after"] = ros_goal_handle.pause_after / 1000.0
- # waypointsk
- goal_handle["waypoints"] = [x for x in ros_goal_handle.waypoints]
+ # waypoints
+ goal_handle["waypoints"] = [str(x) for x in ros_goal_handle.waypoints]
return goal_handle
@@ -165,7 +166,7 @@ def task_queue_task_from_ros(ros_task_queue_task):
return {
'id': ros_task_queue_task.id,
'name': ros_task_queue_task.name,
- 'vid': ros_task_queue_task.vid,
+ 'vid': str(ros_task_queue_task.vid),
}
diff --git a/main/src/vtr_gui/vtr_gui/socket_server.py b/main/src/vtr_gui/vtr_gui/socket_server.py
index a6977da3d..6887b6284 100755
--- a/main/src/vtr_gui/vtr_gui/socket_server.py
+++ b/main/src/vtr_gui/vtr_gui/socket_server.py
@@ -197,6 +197,7 @@ def handle_task_queue_update(json):
socketio.emit(u"task_queue/update", task_queue_update)
+
def main():
logger.info("Launching the socket server.")
socketio.run(app, host=SOCKET_ADDRESS, port=SOCKET_PORT, use_reloader=False)
diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js
index ceb5922ef..0bd24b930 100644
--- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js
+++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js
@@ -53,6 +53,25 @@ const ANNOTATE_LINE_COLOR = "#000000";
const ANNOTATE_LINE_WIDTH = 0.5;
const GRAPH_OPACITY = 0.9;
+const ID_COLORS = [
+ "#3cb44b", // 0 - mr_green
+ "#911eb4", // 1 - prof_plum
+ "#ffe119", // 2 - col_mustard
+ "#4363d8", // 3 - mrs_peacock
+ "#e6194b", // 4 - red
+ "#f58231", // 5 - orange
+ "#bfef45", // 6 - lime
+ "#42d4f4", // 7 - cyan
+ "#f032e6", // 8 - magenta
+ "#fabebe", // 9 - pink
+ "#ffd8b1", // 10 - apricot
+ "#fffac8", // 11 - cream
+ "#aaffc3", // 12 - mint
+ "#a9a9a9", // 13 - grey
+ "#ffffff", // 14 - white
+ "#000000", // 15 - black
+];
+
/// robot constants
const ROBOT_OPACITY = 0.8;
const ROBOT_UNLOCALIZED_OPACITY = 0.4;
@@ -187,6 +206,8 @@ class GraphMap extends React.Component {
// waypoint marker generator
this.WaypointMarkers = this.displayWaypointMarkers.bind(this);
+
+ this.update_buffer = [];
}
componentDidMount() {
@@ -232,7 +253,7 @@ class GraphMap extends React.Component {
>
{
this.fixed_routes.forEach((route) => {
route.polyline.setStyle({
- color: ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length],
+ color: ID_COLORS[this.getModalRobotId(route.ids) % ID_COLORS.length],
weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
lineCap: "butt",
@@ -447,7 +468,7 @@ class GraphMap extends React.Component {
});
this.active_routes.forEach((route) => {
route.polyline.setStyle({
- color: ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length],
+ color: ID_COLORS[this.getModalRobotId(route.ids) % ID_COLORS.length],
weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
lineCap: "butt",
@@ -455,18 +476,18 @@ class GraphMap extends React.Component {
});
}
- /** @brief Leaflet map creationg callback */
+ /** @brief Leaflet map creation callback */
mapCreatedCallback(map) {
console.debug("Leaflet map created.");
- //
+
this.map = map;
this.map.createPane("graph"); // used for the graph polylines and robot marker
map.getPane("graph").style.zIndex = 500; // same as the shadow pane (polylines default)
- //
+
+ this.graphLayerGroup = L.layerGroup().addTo(this.map);
+
this.map.on("click", this.handleMapClick.bind(this));
- //
this.map.on("zoomend", this.zoomEnd, this);
- //
this.fetchGraphState(true);
}
@@ -488,10 +509,94 @@ class GraphMap extends React.Component {
else return { target: null, distance: max_dist };
}
+ getRobotId(vertexId) {
+ // Re-derive the hex ID the same way genDefaultWaypointName does
+ let n = vertexId.toString(), hexId = '';
+ while (n !== '0' && n !== '') {
+ let r = 0, q = '';
+ for (const d of n) { const c = r * 10 + +d; const qd = Math.floor(c / 16); r = c % 16; if (q || qd) q += qd; }
+ hexId = r.toString(16) + hexId;
+ n = q || '0';
+ }
+ const fullHex = (hexId || '0').padStart(16, '0');
+ return parseInt(fullHex.substring(0, 1), 16); // first 4 bits = first hex digit (0–15)
+ }
+
+ getModalRobotId(ids) {
+ if (!ids || ids.length === 0) return 0;
+
+ const idCounts = new Map();
+ ids.forEach((id) => {
+ const robotId = this.getRobotId(id);
+ idCounts.set(robotId, (idCounts.get(robotId) || 0) + 1);
+ });
+
+ // CHANGE to >= so that in a tie, the newer vertex dictates the color
+ return [...idCounts.entries()].reduce((a, b) => (b[1] >= a[1] ? b : a), [0, 0])[0];
+ }
+
+ splitRoutesByRobotId(routes) {
+ const unbatched = [];
+ routes.forEach(route => {
+ let current_ids = [];
+ let current_robot_id = null;
+
+ for (let i = 0; i < route.ids.length; i++) {
+ let id = route.ids[i];
+ let r_id = this.getRobotId(id);
+
+ if (current_ids.length === 0) {
+ current_ids.push(id);
+ current_robot_id = r_id;
+ } else {
+ // If the robot ID stays the same, continue the segment
+ if (r_id === current_robot_id) {
+ current_ids.push(id);
+ } else {
+ // Robot ID changed! Push the completed segment
+ if (current_ids.length > 0) {
+ unbatched.push({ type: route.type, ids: current_ids });
+ }
+
+ // Start a new segment bridging from the previous vertex so there is no visual gap
+ const prev_id = route.ids[i - 1];
+ const a = prev_id < id ? `${prev_id},${id}` : `${id},${prev_id}`;
+ const connected = this.adjacency?.has(a) ?? true;
+
+ if (connected) {
+ current_ids = [prev_id, id];
+ } else {
+ current_ids = [id];
+ }
+ current_robot_id = r_id;
+ }
+ }
+ }
+ if (current_ids.length > 0) {
+ unbatched.push({ type: route.type, ids: current_ids });
+ }
+ });
+ return unbatched;
+ }
+
genDefaultWaypointName(id) {
- let vl = parseInt(id % Math.pow(2, 32));
- let vh = parseInt((id - vl) / Math.pow(2, 32));
- return "WP-" + vh.toString() + "-" + vl.toString();
+ // Convert decimal string to hex via long division (avoids float64 precision loss)
+ let n = id.toString(), hexId = '';
+ while (n !== '0' && n !== '') {
+ let r = 0, q = '';
+ for (const d of n) { const c = r * 10 + +d; const qd = Math.floor(c / 16); r = c % 16; if (q || qd) q += qd; }
+ hexId = r.toString(16) + hexId;
+ n = q || '0';
+ }
+ var fullHex = (hexId || '0').padStart(16, '0');
+
+ var vhHex = fullHex.substring(0, 12);
+ var vlHex = fullHex.substring(12);
+
+ var vh = parseInt(vhHex, 16);
+ var vl = parseInt(vlHex, 16);
+
+ return "WP-" + vh.toString() + "-" + vl.toString();
}
handleMapClick(e) {
@@ -543,35 +648,97 @@ class GraphMap extends React.Component {
});
}
+ flushUpdateBuffer() {
+ console.info(`Flushing ${this.update_buffer.length} buffered updates.`);
+ if (this.update_buffer.length > 0) {
+ this.update_buffer.forEach(update => this.graphUpdateCallback(update));
+ this.update_buffer = [];
+ }
+ }
+
graphStateCallback(graph_state) {
console.info("Received graph state: ", graph_state);
this.loadGraphState(graph_state);
+ this.flushUpdateBuffer();
}
/** @brief Helper function to convert a pose graph route to a leaflet polyline, and add it to map */
route2Polyline(route) {
- // fixed_routes format: [{type: 0, ids: [id, ...]}, ...]
- let color = ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length];
- let latlngs = route.ids.map((id) => {
+ // Guard against empty routes
+ if (!route || !route.ids || route.ids.length === 0) {
+ return L.layerGroup();
+ }
+
+ // find the modal robot_id
+ const color = ID_COLORS[this.getModalRobotId(route.ids) % ID_COLORS.length];
+ const weight = this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]);
+ const opacity = ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length];
+ const style = { color: color, weight: weight, opacity: opacity, pane: "graph", lineCap: "butt" };
+
+ let segments = [];
+ let current = [];
+
+ for (let i = 0; i < route.ids.length; i++) {
+ let id = route.ids[i];
let v = this.id2vertex.get(id);
- return [v.lat, v.lng];
- });
- let polyline = L.polyline(latlngs, {
- color: color,
- weight: this.metres2pix(ROUTE_TYPE_WIDTH[route.type % ROUTE_TYPE_COLOR.length]),
- opacity: ROUTE_TYPE_OPACITY[route.type % ROUTE_TYPE_COLOR.length],
- pane: "graph",
- lineCap: "butt",
- });
- polyline.addTo(this.map);
- return polyline;
+ if (!v) { current = []; continue; }
+
+ if (current.length === 0) {
+ current.push([v.lat, v.lng]);
+ } else {
+ let prev_id = route.ids[i - 1];
+ let prev_v = this.id2vertex.get(prev_id);
+ const a = prev_id < id ? `${prev_id},${id}` : `${id},${prev_id}`;
+ const connected = this.adjacency?.has(a) ?? true; // fallback true during live streaming
+ if (connected) {
+ current.push([v.lat, v.lng]);
+ } else {
+ if (current.length > 1) segments.push(current);
+ current = [[v.lat, v.lng]];
+ }
+ }
+ }
+ if (current.length > 1) segments.push(current);
+
+ const group = L.layerGroup();
+ segments.forEach(seg => L.polyline(seg, style).addTo(group));
+ // group.addTo(this.map);
+
+ if (this.graphLayerGroup) {
+ group.addTo(this.graphLayerGroup);
+ }
+
+ group.setStyle = (newStyle) => {
+ group.eachLayer(layer => layer.setStyle(newStyle));
+ };
+
+ group.addLatLng = ([lat, lng]) => {
+ const layers = Object.values(group._layers);
+ if (layers.length === 0) {
+ L.polyline([[lat, lng]], style).addTo(group);
+ } else {
+ layers[layers.length - 1].addLatLng([lat, lng]);
+ }
+ };
+
+ // const _remove = group.remove.bind(group);
+ // group.remove = () => {
+ // group.clearLayers();
+ // _remove();
+ // };
+
+ return group;
}
/** @brief Refresh the pose graph completely */
loadGraphState(graph, center = false) {
console.info("Loading the current pose graph state (full).");
- // root vid
- this.root_vid = 0; // = graph.root_vid; /// \todo
+ this.root_vid = graph.root_vid;
+
+ if (this.graphLayerGroup) {
+ this.graphLayerGroup.clearLayers();
+ }
+
// id2vertex and kdtree
this.id2vertex = new Map();
let wps_map = new Map();
@@ -590,27 +757,39 @@ class GraphMap extends React.Component {
} else {
wps_map.set(v.id, this.genDefaultWaypointName(v.id));
}
+ });
+ this.adjacency = new Set();
+ graph.vertices.forEach((v) => {
+ v.neighbors.forEach((n) => {
+ // Store both directions as "smallId,largeId" for O(1) lookup
+ const key = v.id < n ? `${v.id},${n}` : `${n},${v.id}`;
+ this.adjacency.add(key);
+ });
});
- this.setState({waypoints_map: wps_map, display_waypoints_map: disp_wps_map});
+ this.setState({waypoints_map: wps_map, display_waypoints_map: disp_wps_map});
this.kdtree = new kdTree(graph.vertices, (a, b) => b.distanceTo(a), ["lat", "lng"]);
- // fixed routes
- this.fixed_routes.forEach((route) => {
- route.polyline.remove();
- });
- this.fixed_routes = graph.fixed_routes.flatMap((route) => {
+
+ // this.active_routes.forEach((route) => route.polyline.remove());
+ // this.active_routes = [];
+ // this.robot_routes = new Map();
+
+ this.active_routes = [];
+ this.fixed_routes = [];
+ this.robot_routes = new Map();
+
+// fixed routes
+ this.fixed_routes = this.splitRoutesByRobotId(graph.fixed_routes).flatMap((route) => {
let polyline = this.route2Polyline(route);
let route_centre = structuredClone(route);
route_centre.type = 7;
let polyline_centre = this.route2Polyline(route_centre);
return [{ ...route, polyline: polyline}, {...route_centre, polyline: polyline_centre}];
});
+
// active routes
- this.active_routes.forEach((route) => {
- route.polyline.remove();
- });
- this.active_routes = graph.active_routes.flatMap((route) => {
+ this.active_routes = this.splitRoutesByRobotId(graph.active_routes).flatMap((route) => {
let polyline = this.route2Polyline(route);
let route_centre = structuredClone(route);
route_centre.type = 7;
@@ -618,6 +797,16 @@ class GraphMap extends React.Component {
return [{ ...route, polyline: polyline}, {...route_centre, polyline: polyline_centre}];
});
+ // active_routes are stored in pairs [route, route_centre], step by 2
+ for (let i = 0; i < this.active_routes.length; i += 2) {
+ const route = this.active_routes[i];
+ const route_centre = this.active_routes[i + 1];
+ if (route && route.ids.length > 0) {
+ const robot_id = this.getRobotId(route.ids[route.ids.length - 1]);
+ this.robot_routes.set(robot_id, { route, route_centre });
+ }
+ }
+
// center set to root vertex
if (center && this.id2vertex.has(this.root_vid)) {
let v = this.id2vertex.get(this.root_vid);
@@ -764,50 +953,59 @@ class GraphMap extends React.Component {
graphUpdateCallback(graph_update) {
if (this.map === null) return;
- if (this.graph_loaded === false) return;
- console.info("Received graph update: ", graph_update);
-
+ if (this.graph_loaded === false) {
+ console.warn("Graph not ready, buffering update for vertex:", graph_update.vertex_to.id);
+ this.update_buffer.push(graph_update);
+ return;
+ }
+ console.info("Processing graph update: ", graph_update);
+
// from vertex
let vf = graph_update.vertex_from;
vf.valueOf = () => vf.id;
vf.distanceTo = L.LatLng.prototype.distanceTo;
- // only update if the vertex is not in the map (vertex position does not change)
- if (!this.id2vertex.has(vf.id)) this.kdtree.insert(vf);
- // always update the vertex map, because vertex neighbors may change
- this.id2vertex.set(vf.id, vf);
-
+
// to vertex
let vt = graph_update.vertex_to;
vt.valueOf = () => vt.id;
vt.distanceTo = L.LatLng.prototype.distanceTo;
- // only update if the vertex is not in the map (vertex position does not change)
- if (!this.id2vertex.has(vt.id)) this.kdtree.insert(vt);
- // always update the vertex map, because vertex neighbors may change
+
+ if (!this.id2vertex.has(vf.id)) {
+ if (this.kdtree) this.kdtree.insert(vf);
+ }
+ this.id2vertex.set(vf.id, vf);
+
+ if (!this.id2vertex.has(vt.id)) {
+ if (this.kdtree) this.kdtree.insert(vt);
+ }
this.id2vertex.set(vt.id, vt);
+
+ // top 4 bits
+ const robot_id = this.getRobotId(vf.id);
+
+ const existing = this.robot_routes.get(robot_id);
+ const tail_id = existing?.route.ids.at(-1);
+ const typeChanged = existing && existing.route.type !== vt.type;
+ const needNewSegment = !existing || tail_id?.toString() !== vf.id.toString() || typeChanged;
+
+ if (needNewSegment) {
+ let route = { ids: [vf.id, vt.id], type: vt.type };
+ route.polyline = this.route2Polyline(route);
+
+ let route_centre = { ids: [vf.id, vt.id], type: 7 };
+ route_centre.polyline = this.route2Polyline(route_centre);
- // active route update
- if (this.active_routes.length === 0) {
- let active_route = { ids: [vf.id], type: vf.type };
- active_route = { ...active_route, polyline: this.route2Polyline(active_route) };
- this.active_routes.push(active_route);
- let active_route_centre = { ids: [vf.id], type: 7 };
- active_route_centre = { ...active_route_centre, polyline: this.route2Polyline(active_route_centre) };
- this.active_routes.push(active_route_centre);
- }
- let active_route = this.active_routes[this.active_routes.length - 2];
- active_route.ids.push(vt.id);
- active_route.polyline.addLatLng([vt.lat, vt.lng]);
- let active_route_centre = this.active_routes[this.active_routes.length - 1];
- active_route_centre.ids.push(vt.id);
- active_route_centre.polyline.addLatLng([vt.lat, vt.lng]);
- if (active_route.type !== vt.type) {
- let new_active_route = { ids: [vt.id], type: vt.type };
- new_active_route = { ...new_active_route, polyline: this.route2Polyline(new_active_route) };
- this.active_routes.push(new_active_route);
- let new_active_route_centre = { ids: [vf.id], type: 7 };
- new_active_route_centre = { ...new_active_route_centre, polyline: this.route2Polyline(new_active_route_centre) };
- this.active_routes.push(new_active_route_centre);
+ this.active_routes.push(route, route_centre);
+ this.robot_routes.set(robot_id, { route: route, route_centre: route_centre });
+ } else {
+ // Extend the existing segment cleanly
+ const {route, route_centre} = existing;
+ route.ids.push(vt.id);
+ route.polyline.addLatLng([vt.lat, vt.lng]);
+ route_centre.ids.push(vt.id);
+ route_centre.polyline.addLatLng([vt.lat, vt.lng]);
}
+ console.info("Graph update completed: ", graph_update);
}
/**
@@ -1044,7 +1242,7 @@ class GraphMap extends React.Component {
// set up and add the center marker
let closest_vertices = this.kdtree.nearest(center_pos, 1);
selector.vertex.c = closest_vertices ? closest_vertices[0][0] : center_pos;
- selector.vertex.c = this.id2vertex.get(0);
+ selector.vertex.c = this.id2vertex.get(this.root_vid) || this.id2vertex.values().next().value;
selector.marker.c = L.marker(selector.vertex.c, {
draggable: true,
icon: SELECTOR_CENTER_ICON,
@@ -1535,7 +1733,9 @@ class GraphMap extends React.Component {
}
metres2pix(metres) {
- let origin = L.latLng(this.id2vertex.get(this.root_vid));
+ let ref = this.id2vertex.get(this.root_vid) || this.id2vertex.values().next().value;
+ if (!ref) return metres;
+ let origin = L.latLng(ref);
let origin_plus_metre = this.map.latLngToLayerPoint(L.latLng(origin.lat - 1, origin.lng));
let metre_standard = origin_plus_metre.subtract(this.map.latLngToLayerPoint(origin));
let factor = metre_standard.y * 0.000009;
diff --git a/main/src/vtr_mission_planning/include/vtr_mission_planning/test_utils.hpp b/main/src/vtr_mission_planning/include/vtr_mission_planning/test_utils.hpp
index 8f21f9227..aa4b7db8e 100644
--- a/main/src/vtr_mission_planning/include/vtr_mission_planning/test_utils.hpp
+++ b/main/src/vtr_mission_planning/include/vtr_mission_planning/test_utils.hpp
@@ -57,8 +57,8 @@ struct TestTactic : public StateMachine::Tactic {
void setForceAddVertex(const bool) override {}
void setTrunk(const tactic::VertexId&) override {}
/// Called when trying to merge into existing path
- void connectToTrunk(const bool privileged) override {
- LOG(WARNING) << "Connecting to trunk with privileged " << privileged;
+ void connectToTrunk(const pose_graph::EdgeMode& edge_mode) override {
+ LOG(WARNING) << "Connecting to trunk with edge mode " << static_cast(edge_mode);
}
tactic::Localization getPersistentLoc() const override { return loc_; }
diff --git a/main/src/vtr_mission_planning/src/state_machine/states/localize/metric_localize.cpp b/main/src/vtr_mission_planning/src/state_machine/states/localize/metric_localize.cpp
index 25558158a..0e5d5d57e 100644
--- a/main/src/vtr_mission_planning/src/state_machine/states/localize/metric_localize.cpp
+++ b/main/src/vtr_mission_planning/src/state_machine/states/localize/metric_localize.cpp
@@ -59,7 +59,7 @@ void MetricLocalize::onExit(StateMachine &state_machine,
// leaves to root
auto tactic = getTactic(state_machine);
- tactic->connectToTrunk(false);
+ tactic->connectToTrunk(pose_graph::EdgeMode::Autonomous);
tactic->setPath(PathType(), 0, tactic::EdgeTransform(true), false);
diff --git a/main/src/vtr_mission_planning/src/state_machine/states/teach/merge.cpp b/main/src/vtr_mission_planning/src/state_machine/states/teach/merge.cpp
index e34c6f019..e505e7d24 100644
--- a/main/src/vtr_mission_planning/src/state_machine/states/teach/merge.cpp
+++ b/main/src/vtr_mission_planning/src/state_machine/states/teach/merge.cpp
@@ -66,7 +66,7 @@ void Merge::onExit(StateMachine &state_machine, StateInterface &new_state) {
// Note: This is called *before* we call up the tree, as we destruct from
// leaves to root
const auto tactic = getTactic(state_machine);
- if (closure_required_) tactic->connectToTrunk(true);
+ if (closure_required_) tactic->connectToTrunk(pose_graph::EdgeMode::Manual);
tactic->setPath(PathType(), 0, tactic::EdgeTransform(true), true);
tactic->setTrunk(); // reset target loc only
diff --git a/main/src/vtr_mission_planning/src/state_machine/states/teach/metric_localize.cpp b/main/src/vtr_mission_planning/src/state_machine/states/teach/metric_localize.cpp
index 102d0c851..6d0819c03 100644
--- a/main/src/vtr_mission_planning/src/state_machine/states/teach/metric_localize.cpp
+++ b/main/src/vtr_mission_planning/src/state_machine/states/teach/metric_localize.cpp
@@ -59,7 +59,7 @@ void MetricLocalize::onExit(StateMachine &state_machine,
// leaves to root
if (localization_required_) {
const auto tactic = getTactic(state_machine);
- tactic->connectToTrunk(true);
+ tactic->connectToTrunk(pose_graph::EdgeMode::Manual);
tactic->setPath(PathType(), 0, tactic::EdgeTransform(true), false);
}
diff --git a/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp b/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp
index 1e08002c3..6a7bdc5a8 100644
--- a/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp
+++ b/main/src/vtr_navigation/include/vtr_navigation/graph_map_server.hpp
@@ -117,7 +117,8 @@ class GraphMapServer : public tactic::Graph::Callback,
void poseCallback(const NavSatFix::ConstSharedPtr msg);
void updateWaypointCallback(const UpdateWaypointMsg::ConstSharedPtr msg);
void updateMissionCallback(const MissionCommandMsg::ConstSharedPtr msg);
-
+ void refreshGraphState();
+
private:
/// these functions are called with graph mutex locked
void vertexAdded(const VertexPtr& v) override;
@@ -133,16 +134,21 @@ class GraphMapServer : public tactic::Graph::Callback,
/// these functions are called by functions above, do not lock mutex inside
/** \brief Helper to get a shared pointer to the graph */
GraphPtr getGraph() const;
+
/** \brief Returns a privileged graph (only contains teach routes) */
GraphBasePtr getPrivilegedGraph() const;
+ GraphBasePtr getTopologyGraph() const;
/** \brief Compute graph in a privileged frame, changes vid2tf_map_ */
void optimizeGraph(const GraphBasePtr& priv_graph);
void updateVertexProjection();
void updateVertexType();
void updateVertexName();
void computeRoutes(const GraphBasePtr& priv_graph);
+ void buildAndPublishGraphState();
+
/** \brief Update the graph incrementally when no optimization is needed */
bool updateIncrementally(const EdgePtr& e);
+ bool publishUpdate(const EdgePtr& e);
void updateRobotProjection();
@@ -213,6 +219,10 @@ class GraphMapServer : public tactic::Graph::Callback,
const float dist_thres_ = 0.1;
bool initial_pose_set_ = false;
std::deque> gps_coords_ {2};
+
+ rclcpp::TimerBase::SharedPtr update_flush_timer_;
+ rclcpp::TimerBase::SharedPtr graph_state_refresh_timer_;
+ std::vector pending_updates_;
};
class RvizGraphMapServer : public GraphMapServer,
diff --git a/main/src/vtr_navigation/src/graph_map_server.cpp b/main/src/vtr_navigation/src/graph_map_server.cpp
index 7333e6d09..4d7df7bf3 100644
--- a/main/src/vtr_navigation/src/graph_map_server.cpp
+++ b/main/src/vtr_navigation/src/graph_map_server.cpp
@@ -82,8 +82,7 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node,
following_route_pub_ = node->create_publisher("following_route", 10);
following_route_srv_ = node->create_service("following_route_srv", std::bind(&GraphMapServer::followingRouteSrvCallback, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_);
-
- // graph manipulation
+ // graph manipulation
auto sub_opt = rclcpp::SubscriptionOptions();
sub_opt.callback_group = callback_group_;
annotate_route_sub_ = node->create_subscription("annotate_route", rclcpp::QoS(10), std::bind(&GraphMapServer::annotateRouteCallback, this, std::placeholders::_1), sub_opt);
@@ -99,7 +98,7 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node,
if (!map_info.set) {
CLOG(INFO, "navigation.graph_map_server")
<< "Initializing pose graph mapinfo";
- map_info.root_vid = 0;
+ map_info.root_vid = -1;
map_info.lng = lng;
map_info.lat = lat;
map_info.theta = theta;
@@ -107,19 +106,35 @@ void GraphMapServer::start(const rclcpp::Node::SharedPtr& node,
map_info.set = true;
graph->setMapInfo(map_info);
}
+
+ CLOG(DEBUG, "navigation.graph_map_server")
+ << "MapInfo set";
//map info
map_info_srv_ = node->create_service("map_info_srv", std::bind(&GraphMapServer::mapInfoSrvCallback, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_);
+ CLOG(DEBUG, "navigation.graph_map_server") << "graph_map_server::start num vtxs";
if (graph->numberOfVertices() == 0) return;
+ CLOG(DEBUG, "navigation.graph_map_server") << "graph_map_server::start graph guard";
+ buildAndPublishGraphState();
+}
- auto graph_lock = graph->guard(); // lock graph then internal lock
+void GraphMapServer::buildAndPublishGraphState() {
+ auto graph_lock = getGraph()->guard();
UniqueLock lock(mutex_);
- const auto priv_graph = getPrivilegedGraph();
- optimizeGraph(priv_graph);
- updateVertexProjection();
- updateVertexType();
- updateVertexName();
+
+ CLOG(DEBUG, "navigation.graph_map_server") << "buildAndPublishGraphState()";
+ auto saved_active_routes = graph_state_.active_routes; // persist active
+ const auto priv_graph = getTopologyGraph();
+ optimizeGraph(priv_graph); // sets graph_state_
+ updateVertexProjection();
+ updateVertexType();
+ updateVertexName();
computeRoutes(priv_graph);
+ graph_state_.active_routes = saved_active_routes;
+
+ if (graph_state_pub_) {
+ graph_state_pub_->publish(graph_state_);
+ }
}
void GraphMapServer::graphStateSrvCallback(
@@ -184,14 +199,7 @@ void GraphMapServer::annotateRouteCallback(
env_info.terrain_type = msg->type;
locked_env_info_msg.setData(env_info);
}
- //
- auto graph_lock = graph->guard(); // lock graph then internal lock
- UniqueLock lock(mutex_);
- const auto priv_graph = getPrivilegedGraph();
- updateVertexType();
- computeRoutes(priv_graph);
- //
- graph_state_pub_->publish(graph_state_);
+ buildAndPublishGraphState();
}
void GraphMapServer::moveGraphCallback(const MoveGraphMsg::ConstSharedPtr msg) {
@@ -209,12 +217,7 @@ void GraphMapServer::moveGraphCallback(const MoveGraphMsg::ConstSharedPtr msg) {
CLOG(DEBUG, "navigation.graph_map_server")
<< "Updated graph map info: <" << map_info.lng << ", " << map_info.lat
<< ", " << map_info.theta << ", " << map_info.scale << ">";
-
- UniqueLock lock(mutex_);
- updateVertexProjection();
- updateRobotProjection();
- //
- graph_state_pub_->publish(graph_state_);
+ buildAndPublishGraphState();
}
float GraphMapServer::haversineDist(float lat1, float lat2, float lon1, float lon2) {
@@ -253,7 +256,6 @@ void GraphMapServer::poseCallback(const NavSatFix::ConstSharedPtr msg) {
auto delta_lng = deltaLongToMetres(prev_coords.second, msg->latitude, prev_coords.first, msg->longitude);
auto delta_lat = deltaLatToMetres(prev_coords.second, msg->latitude);
- map_info.root_vid = 0;
map_info.lng = (float) msg->longitude;
map_info.lat = (float) msg->latitude;
map_info.theta = (float) atan2(delta_lat, delta_lng);
@@ -268,7 +270,6 @@ void GraphMapServer::updateWaypointCallback(
CLOG(DEBUG, "navigation.graph_map_server")
<< "Received update waypoint request: vertex_id:" << msg->vertex_id << ", type:"
<< (int)msg->type << ", name:" << msg->name;
-
const auto graph = getGraph();
{
@@ -280,7 +281,8 @@ void GraphMapServer::updateWaypointCallback(
if (waypoint_name_msg == nullptr) {
CLOG(ERROR, "navigation.graph_map_server")
<< "Failed to retrieve waypoint_name for vertex " << msg->vertex_id;
- throw std::runtime_error{"Failed to retrieve waypoint_name for vertex"};
+ return;
+ // throw std::runtime_error{"Failed to retrieve waypoint_name for vertex"};
}
auto locked_waypoint_name_msg_ref = waypoint_name_msg->locked(); // lock the msg
auto& locked_waypoint_name_msg = locked_waypoint_name_msg_ref.get();
@@ -299,23 +301,17 @@ void GraphMapServer::updateWaypointCallback(
locked_waypoint_name_msg.setData(waypoint_name);
}
-
- auto graph_lock = graph->guard(); // lock graph then internal lock
- UniqueLock lock(mutex_);
- updateVertexName();
- graph_state_pub_->publish(graph_state_);
-
+ buildAndPublishGraphState();
}
void GraphMapServer::vertexAdded(const VertexPtr& v) {
if (getGraph()->numberOfVertices() > 1) return;
- /// The first vertex is added
- if ((uint64_t)v->id() != 0) {
- std::string err{"First vertex added is not the root vertex"};
- CLOG(ERROR, "navigation.graph_map_server") << err;
- throw std::runtime_error{err};
- };
+ /// The first vertex is added — record it as the root
UniqueLock lock(mutex_);
+ auto graph = getGraph();
+ auto map_info = graph->getMapInfo();
+ map_info.root_vid = (uint64_t)v->id();
+ graph->setMapInfo(map_info);
/// \note \todo currently privileged graph is extracted based on edges
/// (manual/autonomous), at this moment we do not have any edge in the graph
/// so privileged graph returns an empty graph, which is wrong. Solution is to
@@ -332,32 +328,22 @@ void GraphMapServer::vertexAdded(const VertexPtr& v) {
}
void GraphMapServer::edgeAdded(const EdgePtr& e) {
- UniqueLock lock(mutex_);
- if (updateIncrementally(e)) return;
- //
- const auto priv_graph = getPrivilegedGraph();
- optimizeGraph(priv_graph);
- updateVertexProjection();
- updateVertexType();
- updateVertexName();
- computeRoutes(priv_graph);
- //
- graph_state_pub_->publish(graph_state_);
+ CLOG(DEBUG, "navigation.graph_map_server") << "edgeAdded";
+ bool ok;
+ {
+ UniqueLock lock(mutex_);
+ ok = updateIncrementally(e);
+ }
+ if (!ok) {
+ CLOG(DEBUG, "navigation.graph_map_server") << "edgeAdded: buildAndPublishGraphState";
+ buildAndPublishGraphState();
+ }
}
void GraphMapServer::endRun() {
- auto graph_lock = getGraph()->guard(); // lock graph then internal lock
- UniqueLock lock(mutex_);
if (getGraph()->numberOfVertices() <= 1) return;
-
- const auto priv_graph = getPrivilegedGraph();
- optimizeGraph(priv_graph);
- updateVertexProjection();
- updateVertexType();
- updateVertexName();
- computeRoutes(priv_graph);
- //
- graph_state_pub_->publish(graph_state_);
+ CLOG(DEBUG, "navigation.graph_map_server") << "endRun()";
+ buildAndPublishGraphState();
}
void GraphMapServer::robotStateUpdated(const tactic::Localization& persistent,
@@ -413,11 +399,11 @@ void GraphMapServer::pathUpdated(const VertexId::Vector& path) {
}
auto GraphMapServer::getGraph() const -> GraphPtr {
- if (auto graph_acquired = graph_.lock())
+ if (auto graph_acquired = graph_.lock()){
return graph_acquired;
- else {
+ } else {
std::string err{"Graph has expired"};
- CLOG(ERROR, "navigation.graph_map_server") << err;
+ CLOG(ERROR, "navigation.graph_map_server") << err;
throw std::runtime_error(err);
}
return nullptr;
@@ -428,16 +414,31 @@ auto GraphMapServer::getPrivilegedGraph() const -> GraphBasePtr {
const auto graph = getGraph();
using PrivEval = tactic::PrivilegedEvaluator;
auto priv_eval = std::make_shared(*graph);
- return graph->getSubgraph(priv_eval);
+ const auto root_vid = getGraph()->root();
+ return graph->getSubgraph(root_vid, priv_eval);
+}
+
+auto GraphMapServer::getTopologyGraph() const -> GraphBasePtr {
+ // get the current privileged graph
+ const auto graph = getGraph();
+ using TopEval = tactic::TopologyEvaluator;
+ auto top_eval = std::make_shared(*graph);
+ const auto root_vid = getGraph()->root();
+ const auto subgraph = graph->getSubgraph(root_vid, top_eval);
+ CLOG(DEBUG, "navigation.graph_map_server") << "getTopologyGraph: subgraph root" << subgraph->root();
+ return subgraph;
}
void GraphMapServer::optimizeGraph(const tactic::GraphBase::Ptr& priv_graph) {
- const auto map_info = getGraph()->getMapInfo();
- const auto root_vid = VertexId(map_info.root_vid);
+ CLOG(DEBUG, "navigation.graph_map_server") << "optimizeGraph: get root";
+ const auto root_vid = getGraph()->root();
+ CLOG(DEBUG, "navigation.graph_map_server") << "optimizeGraph: posegraph optimizer, root_vid: " << root_vid;
pose_graph::PoseGraphOptimizer optimizer(
priv_graph, root_vid, vid2tf_map_);
+ CLOG(DEBUG, "navigation.graph_map_server") << "optimizeGraph: posegraph opitmizer done";
+
// add pose graph relaxation factors
// default covariance to use
Eigen::Matrix cov(Eigen::Matrix::Identity());
@@ -455,7 +456,10 @@ void GraphMapServer::optimizeGraph(const tactic::GraphBase::Ptr& priv_graph) {
CLOG(WARNING, "navigation.graph_map_server") << "Pose graph relaxation for visualization failed. Falling back back to initial config.";
}
+ CLOG(DEBUG, "navigation.graph_map_server") << "optimizeGraph: PG relaxation";
// update the graph state vertices and idx map
+ CLOG(DEBUG, "navigation.graph_map_server") << "optimizeGraph: update graph state vtxs and idx map";
+ graph_state_.root_vid = (uint64_t)root_vid;
auto& vertices = graph_state_.vertices;
vertices.clear();
vid2idx_map_.clear();
@@ -468,6 +472,7 @@ void GraphMapServer::optimizeGraph(const tactic::GraphBase::Ptr& priv_graph) {
//
vid2idx_map_[it->id()] = vertices.size() - 1;
}
+ CLOG(DEBUG, "navigation.graph_map_server") << "optimizeGraph: done";
}
void GraphMapServer::updateVertexProjection() {
@@ -569,7 +574,8 @@ void GraphMapServer::updateVertexType() {
graph->at(vertex.id)
->retrieve("env_info",
"vtr_tactic_msgs/msg/EnvInfo");
- vertex.type = env_info_msg->sharedLocked().get().getData().terrain_type;
+ if (env_info_msg != nullptr) { vertex.type = env_info_msg->sharedLocked().get().getData().terrain_type; }
+ else { vertex.type = static_cast(8); } // not yet populated
graph->at(vertex.id)->SetTerrainType(vertex.type);
int vertex_type = vertex.type;
CLOG(DEBUG, "navigation.graph_map_server") << "Updating Graph Vertex Type: " << vertex_type;
@@ -584,6 +590,7 @@ void GraphMapServer::updateVertexName() {
graph->at(VertexId(vertex.id))
->retrieve("waypoint_name",
"vtr_tactic_msgs/msg/WaypointName");
+ if (waypoint_name_msg == nullptr) continue; // not yet populated
vertex.name = waypoint_name_msg->sharedLocked().get().getData().name;
}
}
@@ -600,6 +607,7 @@ void GraphMapServer::computeRoutes(const tactic::GraphBase::Ptr& priv_graph) {
for (auto&& route : routes) {
int curr_route_type = -1;
for (auto&& id : route.elements()) {
+ if (vid2idx_map_.count(id) == 0) continue; // skip unindexed vertices
const auto type = graph_state_.vertices[vid2idx_map_.at(id)].type;
// new route
if (curr_route_type == -1) {
@@ -625,111 +633,139 @@ void GraphMapServer::computeRoutes(const tactic::GraphBase::Ptr& priv_graph) {
}
}
//
- graph_state_.active_routes.clear();
+ auto& active_routes = graph_state_.active_routes;
+ // Build set of all vertex IDs now in fixed_routes
+ std::unordered_set fixed_ids;
+ for (auto& r : fixed_routes)
+ for (auto id : r.ids) fixed_ids.insert(id);
+
+ // Remove active routes whose vertices are fully absorbed into fixed_routes;
+ // keep any that still have vertices not yet in the decomposition.
+ active_routes.erase(
+ std::remove_if(active_routes.begin(), active_routes.end(),
+ [&fixed_ids](const auto& route) {
+ return std::all_of(route.ids.begin(), route.ids.end(),
+ [&fixed_ids](uint64_t id) { return fixed_ids.count(id) > 0; });
+ }),
+ active_routes.end()
+ );
}
bool GraphMapServer::updateIncrementally(const EdgePtr& e) {
- // Autonomouse edges do not need to be considered
+ CLOG(DEBUG, "navigation.graph_map_server") << "updateIncrementally: " << *e;
+ // Autonomous edges do not need to be considered
if (e->isAutonomous()) return true;
// Spatial edge always triggers a complete update
if (e->isSpatial()) return false;
+
+ //
+ const auto from = e->from();
+ const auto to = e->to();
+ const auto T_to_from = e->T();
+
// Simply ignore this edge if it is not connected to the main graph (trunk)
- if (vid2tf_map_.count(e->from()) == 0) {
+ if (vid2tf_map_.count(from) == 0) {
std::stringstream ss;
- ss << "Cannot find vertex " << e->from()
+ ss << "Cannot find vertex " << from
<< " in vid2tf_map_, haven't localized to a trunk vertex yet so not "
"updating the map.";
CLOG(DEBUG, "navigation.graph_map_server") << ss.str();
return true;
}
- //
- const auto from = e->from();
- const auto to = e->to();
- const auto T_to_from = e->T();
-
// consistency check
if (vid2tf_map_.count(to) != 0) {
std::stringstream ss;
ss << "Cannot connect to an existing vertex " << to
- << " via a temporal edge";
+ << " via a temporal edge from " << from;
CLOG(ERROR, "navigation.graph_map_server") << ss.str();
throw std::runtime_error{ss.str()};
}
- if (!(((uint64_t(to) - uint64_t(from)) == 1))) {
- std::stringstream ss;
- ss << "Temporal edge from " << from << " to " << to << " isn't continuous.";
- CLOG(ERROR, "navigation.graph_map_server") << ss.str();
- throw std::runtime_error{ss.str()};
- }
-
// vid2tfmap update
vid2tf_map_[to] = T_to_from * vid2tf_map_.at(from);
+ CLOG(DEBUG, "navigation.graph_map_server") << "Incremental update succeeded";
+ return publishUpdate(e);
+}
- // graph_state_.vertices.
- auto& vertices = graph_state_.vertices;
- // update from neighbors
- vertices[vid2idx_map_.at(from)].neighbors.push_back(to);
- // add to into the vertices
- auto& vertex = vertices.emplace_back();
- vertex.id = to;
- vertex.neighbors.push_back(from);
- vid2idx_map_[to] = vertices.size() - 1;
+bool GraphMapServer::publishUpdate(const EdgePtr& e) {
+ CLOG(DEBUG, "navigation") << "publishUpdate: working on edge" << *e;
+
+ const auto from = e->from();
+ const auto to = e->to();
+ if (vid2idx_map_.count(from) == 0) {
+ CLOG(WARNING, "navigation.graph_map_server")
+ << "Vertex " << from << " not in vid2idx_map_. Triggering full state rebuild.";
+ return false;
+ }
+
+ auto& from_idx = vid2idx_map_.at(from);
+ if (vid2idx_map_.count(to) == 0){
+ auto& vertex = graph_state_.vertices.emplace_back();
+ vertex.id = to;
+ vid2idx_map_[to] = graph_state_.vertices.size() - 1;
+ }
+
// projection
const auto [lng, lat, theta] = project_vertex_(to);
- vertex.lng = lng;
- vertex.lat = lat;
- vertex.theta = theta;
-
+ auto& vertex_msg = graph_state_.vertices[vid2idx_map_.at(to)];
+ vertex_msg.lng = lng;
+ vertex_msg.lat = lat;
+ vertex_msg.theta = theta;
+
+ // Add these lines to print the timestamps to the console/log:
+ const auto v_from = getGraph()->at(from);
+ const auto v_to = getGraph()->at(to);
+
+ CLOG(INFO, "navigation.graph_map_server")
+ << "Vertex FROM: " << from << " | timestamp: " << v_from->vertexTime();
+ CLOG(INFO, "navigation.graph_map_server")
+ << "Vertex TO: " << to << " | timestamp: " << v_to->vertexTime();
+
// vertex type
- if (vertices[vid2idx_map_.at(from)].type == -1) {
+ if (graph_state_.vertices[vid2idx_map_.at(from)].type == -1 || graph_state_.vertices[vid2idx_map_.at(from)].type == 8) {
const auto env_info_msg = getGraph()->at(from)->retrieve(
"env_info", "vtr_tactic_msgs/msg/EnvInfo");
- if (env_info_msg == nullptr) {
- std::stringstream ss;
- ss << "Cannot find env_info for vertex " << from
- << ", which is assumed added at this moment.";
- CLOG(ERROR, "navigation.graph_map_server") << ss.str();
- throw std::runtime_error{ss.str()};
+ if (env_info_msg != nullptr) {
+ graph_state_.vertices[vid2idx_map_.at(from)].type = env_info_msg->sharedLocked().get().getData().terrain_type;
+ } else {
+ CLOG(WARNING, "navigation.graph_map_server") << "Missing env_info for vertex (from) " << from << ", defaulting.";
+ graph_state_.vertices[vid2idx_map_.at(from)].type = -1;
}
- vertices[vid2idx_map_.at(from)].type =
- env_info_msg->sharedLocked().get().getData().terrain_type;
}
- const auto env_info_msg = getGraph()->at(to)->retrieve(
- "env_info", "vtr_tactic_msgs/msg/EnvInfo");
- if (env_info_msg == nullptr) {
- std::stringstream ss;
- ss << "Cannot find env_info for vertex " << to
- << ", which is assumed added at this moment.";
- CLOG(ERROR, "navigation.graph_map_server") << ss.str();
- throw std::runtime_error{ss.str()};
+
+ if (graph_state_.vertices[vid2idx_map_.at(to)].type == -1 || graph_state_.vertices[vid2idx_map_.at(to)].type == 8) {
+ const auto env_info_msg = getGraph()->at(to)->retrieve(
+ "env_info", "vtr_tactic_msgs/msg/EnvInfo");
+ if (env_info_msg != nullptr) {
+ graph_state_.vertices[vid2idx_map_.at(to)].type = env_info_msg->sharedLocked().get().getData().terrain_type;
+ } else {
+ CLOG(WARNING, "navigation.graph_map_server") << "Missing env_info for vertex (to) " << to << ", defaulting.";
+ graph_state_.vertices[vid2idx_map_.at(to)].type = -1;
+ }
}
- vertex.type = env_info_msg->sharedLocked().get().getData().terrain_type;
// add to active route
auto& active_routes = graph_state_.active_routes;
if (active_routes.empty()) {
active_routes.emplace_back();
auto& active_route = active_routes.back();
- active_route.type = vertices[vid2idx_map_.at(from)].type;
- active_route.ids.emplace_back(from);
+ active_route.type = graph_state_.vertices[vid2idx_map_.at(from)].type;
+ active_route.ids.emplace_back(from);
}
active_routes.back().ids.emplace_back(to);
- if (active_routes.back().type != vertex.type) {
+ if (active_routes.back().type != vertex_msg.type) {
active_routes.emplace_back();
auto& active_route = active_routes.back();
- active_route.type = vertex.type;
+ active_route.type = vertex_msg.type;
active_route.ids.emplace_back(to);
}
// compute and publish the update message
GraphUpdate graph_update;
- graph_update.vertex_from = vertices[vid2idx_map_.at(from)];
- graph_update.vertex_to = vertices[vid2idx_map_.at(to)];
+ graph_update.vertex_from = graph_state_.vertices[vid2idx_map_.at(from)];
+ graph_update.vertex_to = graph_state_.vertices[vid2idx_map_.at(to)];
graph_update_pub_->publish(graph_update);
-
- CLOG(DEBUG, "navigation.graph_map_server") << "Incremental update succeeded";
return true;
}
diff --git a/main/src/vtr_navigation/src/navigator.cpp b/main/src/vtr_navigation/src/navigator.cpp
index 4b01dcada..db74918d6 100644
--- a/main/src/vtr_navigation/src/navigator.cpp
+++ b/main/src/vtr_navigation/src/navigator.cpp
@@ -111,7 +111,10 @@ Navigator::Navigator(const rclcpp::Node::SharedPtr& node) : node_(node) {
tactic_ = std::make_shared(Tactic::Config::fromROS(node_), pipeline,
pipeline_output, graph_, graph_map_server_,
std::make_shared(node_));
- if (graph_->contains(VertexId(0, 0))) tactic_->setTrunk(VertexId(0, 0));
+ {
+ const auto root = graph_->root();
+ if (root.isValid()) tactic_->setTrunk(root);
+ }
/// path planner
auto planner_factory = std::make_shared(node_);
diff --git a/main/src/vtr_path_planning/test/closed_loop/unicycle_tests.cpp b/main/src/vtr_path_planning/test/closed_loop/unicycle_tests.cpp
index 8e08d5d93..54c799b1f 100644
--- a/main/src/vtr_path_planning/test/closed_loop/unicycle_tests.cpp
+++ b/main/src/vtr_path_planning/test/closed_loop/unicycle_tests.cpp
@@ -50,7 +50,7 @@ class ChainTest : public TestWithParam {
}
~ChainTest() override {}
- void createConstantCurve(const EdgeTransform& T_wr0, const EdgeTransform& T_wrf, const Timestamp& t1, const Timestamp& t2, unsigned nodes=2, bool is_teach=true) {
+ void createConstantCurve(const EdgeTransform& T_wr0, const EdgeTransform& T_wrf, const Timestamp& t1, const Timestamp& t2, unsigned nodes=2, EdgeMode is_teach=EdgeMode::Manual) {
ASSERT_GE(nodes, 2);
ASSERT_GT(t2, t1);
@@ -80,7 +80,7 @@ class ChainTest : public TestWithParam {
createConstantCurve(tf_from_global(0, 0, 0), tf_from_global(20.0, 0.0, 0), 0, 10*1e9, 20);
createConstantCurve(tf_from_global(20.5, 0, 0), tf_from_global(25.5, 5.0, M_PI_2), 10*1e9, 20*1e9, 20);
graph_->addEdge(VertexId(0, 19), VertexId(0, 20), EdgeType::Temporal,
- true, tf_from_global(0.5, 0.0, 0).inverse());
+ EdgeMode::Manual, tf_from_global(0.5, 0.0, 0).inverse());
using PrivEvaluator = eval::mask::privileged::CachedEval;
@@ -172,7 +172,7 @@ TEST_P(ChainTest, NoiseFreeMPC) {
EdgeTransform T_init = tf_from_global(x_init, y_init, theta_init);
graph_->addVertex(t_offset_);
graph_->addEdge(VertexId(repeat_run, 0), VertexId(0, 0), EdgeType::Spatial,
- false, T_init);
+ EdgeMode::Autonomous, T_init);
ASSERT_GT(chain_->p(chain_->size() - 1), 0);
@@ -229,7 +229,7 @@ TEST_P(ChainTest, NoiseFreeMPC) {
delta_TF.setZeroCovariance();
auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9);
graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal,
- false, delta_TF);
+ EdgeMode::Autonomous, delta_TF);
chain_->updatePetioleToLeafTransform(t_offset_ + (i + 1) * config.DT * 1e9, P_tran * new_velo,
delta_TF, false);
@@ -266,12 +266,12 @@ TEST_P(ChainTest, NoiseFreeMPC) {
uint64_t mpc_rollout_run = graph_->addRun();
graph_->addVertex(t_offset_);
graph_->addEdge(VertexId(mpc_rollout_run, 0), VertexId(0, 0), EdgeType::Spatial,
- false, T_init);
+ EdgeMode::Autonomous, T_init);
for(int i = 0; i < config.N; ++i) {
auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9);
graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal,
- false, mpc_poses[i+1].inverse()*mpc_poses[i]);
+ EdgeMode::Autonomous, mpc_poses[i+1].inverse()*mpc_poses[i]);
}
}
@@ -290,7 +290,7 @@ TEST_P(ChainTest, NoisyMPC) {
EdgeTransform T_init = tf_from_global(x_init, y_init, theta_init);
graph_->addVertex(t_offset_);
graph_->addEdge(VertexId(repeat_run, 0), VertexId(0, 0), EdgeType::Spatial,
- false, T_init);
+ EdgeMode::Autonomous, T_init);
ASSERT_GT(chain_->p(chain_->size() - 1), 0);
@@ -347,7 +347,7 @@ TEST_P(ChainTest, NoisyMPC) {
delta_TF.setZeroCovariance();
auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9);
graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal,
- false, delta_TF);
+ EdgeMode::Autonomous, delta_TF);
chain_->updatePetioleToLeafTransform(t_offset_ + (i + 1) * config.DT * 1e9, P_tran * new_velo,
delta_TF, false);
@@ -384,12 +384,12 @@ TEST_P(ChainTest, NoisyMPC) {
uint64_t mpc_rollout_run = graph_->addRun();
graph_->addVertex(t_offset_);
graph_->addEdge(VertexId(mpc_rollout_run, 0), VertexId(0, 0), EdgeType::Spatial,
- false, T_init);
+ EdgeMode::Autonomous, T_init);
for(int i = 0; i < config.N; ++i) {
auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9);
graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal,
- false, mpc_poses[i+1].inverse()*mpc_poses[i]);
+ EdgeMode::Autonomous, mpc_poses[i+1].inverse()*mpc_poses[i]);
}
}
@@ -410,7 +410,7 @@ TEST_P(RealChainTest, NoiseFreeRealMPC) {
EdgeTransform T_init = tf_from_global(x_init, y_init, theta_init);
graph_->addVertex(t_offset_);
graph_->addEdge(VertexId(repeat_run, 0), VertexId(0, 2), EdgeType::Spatial,
- false, T_init);
+ EdgeMode::Autonomous, T_init);
ASSERT_GT(chain_->p(chain_->size() - 1), 0);
@@ -480,7 +480,7 @@ TEST_P(RealChainTest, NoiseFreeRealMPC) {
delta_TF.setZeroCovariance();
auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9);
graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal,
- false, delta_TF);
+ EdgeMode::Autonomous, delta_TF);
chain_->updatePetioleToLeafTransform(t_offset_ + (i + 1) * config.DT * 1e9, P_tran * new_velo,
delta_TF, false);
@@ -521,12 +521,12 @@ TEST_P(RealChainTest, NoiseFreeRealMPC) {
uint64_t mpc_rollout_run = graph_->addRun();
graph_->addVertex(t_offset_);
graph_->addEdge(VertexId(mpc_rollout_run, 0), VertexId(0, 2), EdgeType::Spatial,
- false, T_init);
+ EdgeMode::Autonomous, T_init);
for(int i = 0; i < config.N; ++i) {
auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9);
graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal,
- false, mpc_poses[i+1].inverse()*mpc_poses[i]);
+ EdgeMode::Autonomous, mpc_poses[i+1].inverse()*mpc_poses[i]);
}
@@ -534,12 +534,12 @@ TEST_P(RealChainTest, NoiseFreeRealMPC) {
auto new_vertex = graph_->addVertex(t_offset_);
auto start_vertex = graph_->addEdge(new_vertex->id(), VertexId(0, 2), EdgeType::Spatial,
- false, target_poses_0.front());
+ EdgeMode::Autonomous, target_poses_0.front());
for(size_t i = 1; i < target_poses_0.size(); i++) {
auto new_vertex = graph_->addVertex(t_offset_ + i * config.DT * 1e9);
graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal,
- false, target_poses_0[i].inverse()*target_poses_0[i-1]);
+ EdgeMode::Autonomous, target_poses_0[i].inverse()*target_poses_0[i-1]);
}
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/evaluators.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/evaluators.hpp
index ef394fae7..eface0736 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/evaluators.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/evaluators.hpp
@@ -25,6 +25,7 @@
#include "vtr_pose_graph/evaluator/mask/direction_from_vertex.hpp"
#include "vtr_pose_graph/evaluator/mask/privileged.hpp"
+#include "vtr_pose_graph/evaluator/mask/topology.hpp"
#include "vtr_pose_graph/evaluator/mask/spatial.hpp"
#include "vtr_pose_graph/evaluator/mask/temporal.hpp"
#include "vtr_pose_graph/evaluator/mask/run_select.hpp"
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/mask/run_select.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/mask/run_select.hpp
index 806a8f0e6..08ceb6509 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/mask/run_select.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/mask/run_select.hpp
@@ -13,7 +13,7 @@
// limitations under the License.
/**
- * \file privileged.hpp
+ * \file run_select.hpp
* \author Yuchen Wu, Autonomous Space Robotics Lab (ASRL)
*/
#pragma once
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/mask/topology.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/mask/topology.hpp
new file mode 100644
index 000000000..f339f9025
--- /dev/null
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/evaluator/mask/topology.hpp
@@ -0,0 +1,113 @@
+// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/**
+ * \file topology.hpp
+ * \author Yuchen Wu, Autonomous Space Robotics Lab (ASRL)
+ */
+#pragma once
+
+#include "vtr_pose_graph/evaluator_base/types.hpp"
+
+namespace vtr {
+namespace pose_graph {
+namespace eval {
+namespace mask {
+namespace topology {
+
+namespace detail {
+
+template
+ReturnType computeVertex(const GRAPH &graph, const VertexId &v) {
+ const auto nbs = graph.neighbors(v);
+ for (const auto &nb : nbs) {
+ if (graph.at(EdgeId(v, nb))->isManual() || graph.at(EdgeId(v, nb))->isUnknown()) return true;
+ }
+ return false;
+}
+
+template
+ReturnType computeEdge(const GRAPH &graph, const EdgeId &e) {
+ return (graph.at(e)->isManual() || graph.at(e)->isUnknown()) ||
+ (computeVertex(graph, e.id1()) && computeVertex(graph, e.id2()));
+}
+
+} // namespace detail
+
+template
+class Eval : public BaseEval {
+ public:
+ PTR_TYPEDEFS(Eval);
+
+ Eval(const GRAPH &graph) : graph_(graph) {}
+
+ protected:
+ ReturnType computeEdge(const EdgeId &e) override {
+ return detail::computeEdge(graph_, e);
+ }
+
+ ReturnType computeVertex(const VertexId &v) override {
+ return detail::computeVertex(graph_, v);
+ }
+
+ private:
+ const GRAPH &graph_;
+};
+
+template
+class CachedEval : public BaseCachedEval {
+ public:
+ PTR_TYPEDEFS(CachedEval);
+
+ CachedEval(const GRAPH &graph) : graph_(graph) {}
+
+ protected:
+ ReturnType computeEdge(const EdgeId &e) override {
+ return detail::computeEdge(graph_, e);
+ }
+
+ ReturnType computeVertex(const VertexId &v) override {
+ return detail::computeVertex(graph_, v);
+ }
+
+ private:
+ const GRAPH &graph_;
+};
+
+template
+class WindowedEval : public BaseWindowedEval {
+ public:
+ PTR_TYPEDEFS(WindowedEval);
+
+ WindowedEval(const GRAPH &graph, const size_t &cache_size)
+ : BaseWindowedEval(cache_size), graph_(graph) {}
+
+ protected:
+ ReturnType computeEdge(const EdgeId &e) override {
+ return detail::computeEdge(graph_, e);
+ }
+
+ ReturnType computeVertex(const VertexId &v) override {
+ return detail::computeVertex(graph_, v);
+ }
+
+ private:
+ const GRAPH &graph_;
+};
+
+} // namespace topology
+} // namespace mask
+} // namespace eval
+} // namespace pose_graph
+} // namespace vtr
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/id/id.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/id/id.hpp
index d1e20eed7..de95905d1 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/id/id.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/id/id.hpp
@@ -14,7 +14,12 @@
/**
* \file base_id.hpp
- * \author Yuchen Wu, Autonomous Space Robotics Lab (ASRL)
+ * \author Yuchen Wu, Anthony Beca, Autonomous Space Robotics Lab (ASRL)
+ *
+ * Vertex ID are assigned according to the following (64-bit) bitfield:
+ * bits 0-3 : ROBOT_ID -> env variable that is unique for each robot (up to 16 robots)
+ * bits 4-44 : run id -> 44-bit UUID that is generated per-run
+ * bits 45-64 : vertex id -> 16-bit sequential incremented with each vertex
*/
#pragma once
@@ -28,6 +33,8 @@ namespace pose_graph {
using BaseIdType = uint32_t;
static constexpr BaseIdType InvalidBaseId = BaseIdType(-1);
+using MajorIdType = uint64_t;
+static constexpr MajorIdType InvalidMajorId = MajorIdType(0x0000FFFFFFFFFFFFull);
using CombinedIdType = uint64_t;
static constexpr CombinedIdType InvalidCombinedId = CombinedIdType(-1);
@@ -38,7 +45,7 @@ class VertexId {
static constexpr VertexId Invalid() { return VertexId(); }
constexpr VertexId() = default;
- constexpr VertexId(const BaseIdType &major_id, const BaseIdType &minor_id)
+ constexpr VertexId(const MajorIdType &major_id, const BaseIdType &minor_id)
: major_id_(major_id), minor_id_(minor_id) {}
constexpr VertexId(const CombinedIdType &combined_id)
: major_id_(upper(combined_id)), minor_id_(lower(combined_id)) {}
@@ -47,7 +54,7 @@ class VertexId {
/** \brief Check if the id is valid */
bool isValid() const {
- return (major_id_ != InvalidBaseId) && (minor_id_ != InvalidBaseId);
+ return (major_id_ != InvalidMajorId) && (minor_id_ != InvalidBaseId);
}
/** \brief Hash operator for use in stl containers */
@@ -69,7 +76,7 @@ class VertexId {
bool operator>=(const VertexId &rhs) const { return !(operator<(rhs)); }
/** \brief Get the run id */
- BaseIdType majorId() const { return major_id_; }
+ MajorIdType majorId() const { return major_id_; }
/** \brief Get the container id, within the run */
BaseIdType minorId() const { return minor_id_; }
@@ -102,20 +109,20 @@ class VertexId {
private:
static constexpr BaseIdType lower(const CombinedIdType &value) {
- return (BaseIdType)(value & 0x00000000FFFFFFFF);
+ return (BaseIdType)(value & 0xFFFF);
}
- static constexpr BaseIdType upper(const CombinedIdType &value) {
- return (BaseIdType)(value >> 32);
+ static constexpr MajorIdType upper(const CombinedIdType &value) {
+ return (MajorIdType)(value >> 16);
}
- static constexpr CombinedIdType combine(const BaseIdType &upper_value,
+ static constexpr CombinedIdType combine(const MajorIdType &upper_value,
const BaseIdType &lower_value) {
- return (CombinedIdType(upper_value) << 32) | CombinedIdType(lower_value);
+ return (CombinedIdType(upper_value) << 16) | CombinedIdType(lower_value & 0xFFFF);
}
protected:
- BaseIdType major_id_ = InvalidBaseId;
+ MajorIdType major_id_ = InvalidMajorId;
BaseIdType minor_id_ = InvalidBaseId;
private:
@@ -160,11 +167,11 @@ class EdgeId {
bool operator>=(const EdgeId &rhs) const { return !operator<(rhs); }
VertexId id1() const { return id_.first; }
- BaseIdType majorId1() const { return id_.first.majorId(); }
+ MajorIdType majorId1() const { return id_.first.majorId(); }
BaseIdType minorId1() const { return id_.first.minorId(); }
VertexId id2() const { return id_.second; }
- BaseIdType majorId2() const { return id_.second.majorId(); }
+ MajorIdType majorId2() const { return id_.second.majorId(); }
BaseIdType minorId2() const { return id_.second.minorId(); }
/** \brief String output */
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/callback_interface.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/callback_interface.hpp
index 315b8ab3b..76d45c8c5 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/callback_interface.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/callback_interface.hpp
@@ -33,6 +33,7 @@ class GraphCallbackInterface {
virtual void vertexAdded(const VertexPtr&) {}
virtual void edgeAdded(const EdgePtr&) {}
+ virtual bool publishUpdate(const EdgePtr&) {}
};
} // namespace pose_graph
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/edge_base.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/edge_base.hpp
index be03a391f..31374a060 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/edge_base.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/edge_base.hpp
@@ -29,6 +29,9 @@ namespace pose_graph {
enum class EdgeType { Temporal = 0, Spatial = 1, Undefined = 2 };
static constexpr size_t NumEdgeType = 2;
+enum class EdgeMode { Autonomous = 0, Manual = 1, Unknown = 2 };
+static constexpr size_t NumEdgeMode = 2;
+
using EdgeTransform = lgmath::se3::TransformationWithCovariance;
class EdgeBase {
@@ -45,11 +48,11 @@ class EdgeBase {
/** \brief Pseudo constructors to generate a shared pointer */
static Ptr MakeShared(const VertexId& from_id, const VertexId& to_id,
- const EdgeType& type, const bool manual,
+ const EdgeType& type, const EdgeMode& mode,
const EdgeTransform& T_to_from = EdgeTransform());
EdgeBase(const VertexId& from_id, const VertexId& to_id, const EdgeType& type,
- const bool manual, const EdgeTransform& T_to_from = EdgeTransform());
+ const EdgeMode& mode, const EdgeTransform& T_to_from = EdgeTransform());
virtual ~EdgeBase() = default;
@@ -74,6 +77,9 @@ class EdgeBase {
/** \brief Return true if the edge was driven autonomously */
bool isAutonomous() const;
+ /** \brief Return true if the edge was driven autonomously */
+ bool isUnknown() const;
+
/** \brief Return true if the edge is a temporal edge */
bool isTemporal() const;
@@ -103,7 +109,7 @@ class EdgeBase {
const EdgeType type_;
/** \brief Whether this edge was manually driven or not */
- const bool manual_;
+ const EdgeMode mode_;
/** \brief protects all non-const class members including: T_to_from_ */
mutable std::shared_mutex mutex_;
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph.hpp
index 85d070cd2..590200032 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph.hpp
@@ -18,6 +18,8 @@
*/
#pragma once
+#include
+
#include "vtr_pose_graph/index/callback_interface.hpp"
#include "vtr_pose_graph/index/graph_base.hpp"
@@ -55,7 +57,7 @@ class Graph : public GraphBase {
Graph(const CallbackPtr& callback = std::make_shared());
/** \brief Add a new run an increment the run id */
- BaseIdType addRun();
+ MajorIdType addRun();
/** \brief Return a blank vertex (current run) with the next available Id */
template
@@ -64,7 +66,7 @@ class Graph : public GraphBase {
/** \brief Return a blank edge with the next available Id */
template
EdgePtr addEdge(const VertexId& from, const VertexId& to,
- const EdgeType& type, const bool manual,
+ const EdgeType& type, const EdgeMode& mode,
const EdgeTransform& T_to_from, Args&&... args);
/** \brief Lock to prevent graph change */
@@ -80,13 +82,14 @@ class Graph : public GraphBase {
using Base::edges_;
- /** \brief The current maximum run index */
- BaseIdType curr_major_id_ = InvalidBaseId;
+ MajorIdType curr_major_id_ = InvalidMajorId;
BaseIdType curr_minor_id_ = InvalidBaseId;
- /** \brief The current maximum run index */
const CallbackPtr callback_;
+ std::mt19937_64 rng_{std::random_device{}()};
+ static uint8_t getRobotIdFromEnv();
+
/**
* \brief Lock by methods that change graph structure, can be used externally
* to prevent graph structure changes during multiple calls to graph methods.
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph.inl b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph.inl
index 4c3c9a1be..5ab1e3613 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph.inl
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph.inl
@@ -32,12 +32,30 @@ template
Graph::Graph(const CallbackPtr& callback) : callback_(callback) {}
template
-BaseIdType Graph::addRun() {
+uint8_t Graph::getRobotIdFromEnv() {
+ const char* env = std::getenv("ROBOT_ID");
+ if (!env) {
+ CLOG(ERROR, "pose_graph") << "ROBOT_ID environment variable not set";
+ throw std::runtime_error("ROBOT_ID environment variable not set");
+ }
+ int id = std::stoi(env);
+ if (id < 0 || id > 15) {
+ CLOG(ERROR, "pose_graph") << "ROBOT_ID out of range (0,15): " << id;
+ throw std::range_error("ROBOT_ID is out of range");
+ }
+ return static_cast(id);
+}
+
+template
+MajorIdType Graph::addRun() {
ChangeGuard change_guard(change_mutex_);
std::unique_lock lock(mutex_);
-
- if ((curr_major_id_ == InvalidBaseId) || (curr_minor_id_ != InvalidBaseId)) {
- ++curr_major_id_;
+ uint8_t robot_id = getRobotIdFromEnv();
+ CLOG(INFO, "pose_graph") << "ROBOT_ID is " << robot_id;
+ if ((curr_major_id_ == InvalidMajorId) || (curr_minor_id_ != InvalidBaseId)) {
+ do {
+ curr_major_id_ = (rng_() & 0x00000FFFFFFFFFFFull) | (static_cast(robot_id) << 44);
+ } while (curr_major_id_ == InvalidMajorId);
curr_minor_id_ = InvalidBaseId;
} else {
CLOG(WARNING, "pose_graph")
@@ -45,7 +63,6 @@ BaseIdType Graph::addRun() {
}
CLOG(DEBUG, "pose_graph") << "Added run " << curr_major_id_;
-
return curr_major_id_;
}
@@ -55,12 +72,13 @@ auto Graph::addVertex(Args&&... args) -> VertexPtr {
ChangeGuard change_guard(change_mutex_);
std::unique_lock lock(mutex_);
- if (curr_major_id_ == InvalidBaseId) {
+ if (curr_major_id_ == InvalidMajorId) {
CLOG(ERROR, "pose_graph") << "No run added";
throw std::runtime_error("No run added");
}
- VertexId vid(curr_major_id_, ++curr_minor_id_);
+ curr_minor_id_ = (curr_minor_id_ == InvalidBaseId) ? 0 : curr_minor_id_ + 1;
+ VertexId vid(curr_major_id_, curr_minor_id_);
graph_.addVertex(vid);
auto vertex = Vertex::MakeShared(vid, std::forward(args)...);
vertices_.insert({vid, vertex});
@@ -76,7 +94,7 @@ auto Graph::addVertex(Args&&... args) -> VertexPtr {
template
template
auto Graph::addEdge(const VertexId& from, const VertexId& to,
- const EdgeType& type, const bool manual,
+ const EdgeType& type, const EdgeMode& mode,
const EdgeTransform& T_to_from, Args&&... args)
-> EdgePtr {
ChangeGuard change_guard(change_mutex_);
@@ -88,19 +106,9 @@ auto Graph::addEdge(const VertexId& from, const VertexId& to,
throw std::range_error("Adding edge between non-existent vertices");
}
- if (from.majorId() < to.majorId()) {
- CLOG(ERROR, "pose_graph")
- << "Cannot add edge from " << from << " to " << to
- << " since the major id of the from vertex is smaller than the to "
- "vertex";
- throw std::invalid_argument(
- "Spatial edges may only be added from higher run numbers to lower "
- "ones");
- }
-
EdgeId eid(from, to);
graph_.addEdge(eid);
- auto edge = Edge::MakeShared(from, to, type, manual, T_to_from,
+ auto edge = Edge::MakeShared(from, to, type, mode, T_to_from,
std::forward(args)...);
edges_.insert({eid, edge});
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph_base.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph_base.hpp
index 4eaad2eb2..bf9701396 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph_base.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/index/graph_base.hpp
@@ -86,6 +86,26 @@ class GraphBase {
return graph_.numberOfEdges();
}
+ /**
+ * \brief Find the root vertex: the run-0 vertex with no incoming temporal
+ * edge. Returns VertexId::Invalid() if the graph is empty.
+ */
+ virtual VertexId root() const {
+ std::shared_lock lock(mutex_);
+ VertexId::Set temporal_to_ids;
+ VertexId::Set privileged_vids;
+ for (const auto& [eid, edge] : edges_) {
+ if (edge->isTemporal()) temporal_to_ids.insert(edge->to());
+ if (edge->isManual()) {
+ privileged_vids.insert(edge->from());
+ privileged_vids.insert(edge->to());
+ }
+ }
+ for (const auto& [vid, vertex] : vertices_)
+ if (privileged_vids.count(vid) && temporal_to_ids.count(vid) == 0) return vid;
+ return VertexId::Invalid();
+ }
+
/** \brief Determine if this graph/subgraph contains a specific vertex */
bool contains(const VertexId& v) const {
std::shared_lock lock(mutex_);
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.inl b/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.inl
index d4c20c25f..90e9783da 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.inl
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.inl
@@ -196,8 +196,8 @@ void LocalizationChain::updateBranchToTwigTransform(
} else if (branch_vid == trunk_vid_) {
return T_branch_trunk_;
} else {
- auto eval =
- std::make_shared>(*this->graph_);
+ auto eval =
+ std::make_shared>(*this->graph_);
auto delta = this->graph_->dijkstraSearch(
branch_vid_, branch_vid,
std::make_shared(1, 1), eval);
@@ -319,11 +319,11 @@ void LocalizationChain::searchClosestTrunk(bool search_backwards) {
trunk_sid_ = best_sid;
trunk_vid_ = this->sequence_[trunk_sid_];
- auto priv_eval =
- std::make_shared>(*this->graph_);
+ auto top_eval =
+ std::make_shared>(*this->graph_);
auto delta = this->graph_->dijkstraSearch(
branch_vid_, trunk_vid_,
- std::make_shared(1, 1), priv_eval);
+ std::make_shared(1, 1), top_eval);
T_branch_trunk_ = eval::ComposeTfAccumulator(
delta->begin(branch_vid_), delta->end(), EdgeTransform(true));
}
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/serializable/rc_edge.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/serializable/rc_edge.hpp
index b5de92264..d94459fb6 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/serializable/rc_edge.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/serializable/rc_edge.hpp
@@ -39,9 +39,9 @@ class RCEdge : public EdgeBase {
using EdgeTransformMsg = vtr_common_msgs::msg::LieGroupTransform;
static Ptr MakeShared(const VertexId& from_id, const VertexId& to_id,
- const EdgeType& type, const bool manual,
+ const EdgeType& type, const EdgeMode& mode,
const EdgeTransform& T_to_from) {
- return std::make_shared(from_id, to_id, type, manual, T_to_from);
+ return std::make_shared(from_id, to_id, type, mode, T_to_from);
}
static Ptr MakeShared(const EdgeMsg& msg,
@@ -50,7 +50,7 @@ class RCEdge : public EdgeBase {
}
RCEdge(const VertexId& from_id, const VertexId& to_id, const EdgeType& type,
- const bool manual, const EdgeTransform& T_to_from);
+ const EdgeMode& mode, const EdgeTransform& T_to_from);
RCEdge(const EdgeMsg& msg,
const storage::LockableMessage::Ptr& msg_ptr);
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/serializable/rc_graph.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/serializable/rc_graph.hpp
index 739764aff..6b8d030aa 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/serializable/rc_graph.hpp
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/serializable/rc_graph.hpp
@@ -26,6 +26,8 @@
#include "vtr_pose_graph_msgs/msg/map_info.hpp"
#include "vtr_pose_graph_msgs/msg/timestamp.hpp"
+#include
+
namespace vtr {
namespace pose_graph {
@@ -75,9 +77,22 @@ class RCGraph : public Graph {
virtual ~RCGraph() { if(!read_only_) save(); }
void save();
+ void saveLive();
/** \brief Return a blank vertex with the next available Id */
VertexPtr addVertex(const Timestamp& time);
+ EdgePtr addEdge(const VertexId& from, const VertexId& to,
+ const EdgeType& type, const EdgeMode& mode,
+ const EdgeTransform& T_to_from);
+
+ /** \brief Returns the root vertex using the stored map_info root_vid */
+ VertexId root() const override {
+ std::shared_lock lock(map_info_mutex_);
+ const auto vid = VertexId(map_info_.root_vid);
+ std::shared_lock graph_lock(mutex_);
+ if (graph_.hasVertex(vid)) return vid;
+ return VertexId::Invalid();
+ }
/** \brief Get the map display calibration */
MapInfoMsg getMapInfo() const {
@@ -87,8 +102,11 @@ class RCGraph : public Graph {
/** \brief Set the map display calibration */
void setMapInfo(const MapInfoMsg& map_info) {
+ CLOG(DEBUG, "pose_graph") << "setMapInfo start";
std::unique_lock lock(map_info_mutex_);
map_info_ = map_info;
+ if (!vertices_.empty()) { map_info_.root_vid = vertices_.begin()->first; }
+ CLOG(DEBUG, "pose_graph") << "setMapInfo done";
}
/** \brief Get the file path of the graph index */
@@ -98,19 +116,30 @@ class RCGraph : public Graph {
template
void write(const std::string& stream_name, const std::string& stream_type,
const typename storage::LockableMessage::Ptr& message);
+ void saveGraphIndex();
+
+ // keep track of topology only edges
+ void loadVerticesLive();
+ void loadEdgesLive();
+ void populateLive();
+ void loadLive();
private:
/** \brief Helper methods for loading from disk */
void loadGraphIndex();
void loadVertices();
void loadEdges();
- void buildSimpleGraph();
+ void buildSimpleGraph();
+
/** \brief Helper methods for saving to disk */
- void saveGraphIndex();
+ // void saveGraphIndex();
void saveVertices();
void saveEdges();
+ void saveVerticesLive();
+ void saveEdgesLive();
+
private:
using Base::mutex_;
@@ -131,6 +160,16 @@ class RCGraph : public Graph {
mutable std::shared_mutex map_info_mutex_;
MapInfoMsg map_info_ = MapInfoMsg();
+
+ std::queue vertices_to_write_;
+ std::queue edges_to_write_;
+ std::unordered_map topology_edges_;
+ std::unordered_map topology_vertices_;
+
+ // keep track of .db3 cursors (watch for new data)
+ int lastVertexIdx_ = 1;
+ int lastEdgeIdx_ = 1;
+
};
template
diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/tools/pose_graph_smoother.inl b/main/src/vtr_pose_graph/include/vtr_pose_graph/tools/pose_graph_smoother.inl
index 12281d3f8..91d694025 100644
--- a/main/src/vtr_pose_graph/include/vtr_pose_graph/tools/pose_graph_smoother.inl
+++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/tools/pose_graph_smoother.inl
@@ -65,7 +65,7 @@ void GraphSmoother::smoothBranch(const EdgeId eid) const
CLOG(DEBUG, "pose_graph.smoothing") << "Found connection between: " << new_edge << " that would be straight";
if (!graph_->contains(new_edge)) {
- graph_->addEdge(branch_id, t_nb, branch->type(), true, T_b_t);
+ graph_->addEdge(branch_id, t_nb, branch->type(), EdgeMode::Manual, T_b_t);
CLOG(INFO, "pose_graph.smoothing") << "Added new edge" << new_edge;
return;
}
diff --git a/main/src/vtr_pose_graph/src/index/edge_base.cpp b/main/src/vtr_pose_graph/src/index/edge_base.cpp
index 9d9713a50..6757dd530 100644
--- a/main/src/vtr_pose_graph/src/index/edge_base.cpp
+++ b/main/src/vtr_pose_graph/src/index/edge_base.cpp
@@ -25,29 +25,20 @@ namespace pose_graph {
EdgeBase::Ptr EdgeBase::MakeShared(const VertexId& from_id,
const VertexId& to_id, const EdgeType& type,
- const bool manual,
+ const EdgeMode& mode,
const EdgeTransform& T_to_from) {
- return std::make_shared(from_id, to_id, type, manual, T_to_from);
+ return std::make_shared(from_id, to_id, type, mode, T_to_from);
}
EdgeBase::EdgeBase(const VertexId& from_id, const VertexId& to_id,
- const EdgeType& type, const bool manual,
+ const EdgeType& type, const EdgeMode& mode,
const EdgeTransform& T_to_from)
: id_(from_id, to_id),
from_(from_id),
to_(to_id),
type_(type),
- manual_(manual),
+ mode_(mode),
T_to_from_(T_to_from) {
- if (from_.majorId() < to_.majorId()) {
- CLOG(ERROR, "pose_graph")
- << "Cannot create edge from " << from_ << " to " << to_
- << " since the major id of the from vertex is smaller than the to "
- "vertex";
- throw std::invalid_argument(
- "Spatial edges may only be added from higher run numbers to lower "
- "ones");
- }
}
EdgeId EdgeBase::id() const { return id_; }
@@ -55,8 +46,9 @@ VertexId EdgeBase::from() const { return from_; }
VertexId EdgeBase::to() const { return to_; }
EdgeType EdgeBase::type() const { return type_; }
size_t EdgeBase::idx() const { return (size_t)type_; }
-bool EdgeBase::isManual() const { return manual_; }
-bool EdgeBase::isAutonomous() const { return !manual_; }
+bool EdgeBase::isManual() const { return mode_ == EdgeMode::Manual; }
+bool EdgeBase::isAutonomous() const { return mode_ == EdgeMode::Autonomous; }
+bool EdgeBase::isUnknown() const { return mode_ == EdgeMode::Unknown; }
bool EdgeBase::isTemporal() const { return type_ == EdgeType::Temporal; }
bool EdgeBase::isSpatial() const { return type_ == EdgeType::Spatial; }
@@ -71,12 +63,7 @@ void EdgeBase::setTransform(const EdgeTransform& T_to_from) {
}
std::ostream& operator<<(std::ostream& out, const EdgeBase& e) {
- if (e.type() == EdgeType::Spatial)
- return out << "{" << e.from() << "--" << e.to() << "}";
- else if (e.type() == EdgeType::Temporal)
- return out << "{" << e.from() << "==" << e.to() << "}";
- else
- return out << "{" << e.from() << "??" << e.to() << "}";
+ return out << "{" << e.from() << "--" << e.to() << "}" << " mode " << static_cast(e.mode_) << " type " << static_cast(e.type_);
}
} // namespace pose_graph
diff --git a/main/src/vtr_pose_graph/src/serializable/rc_edge.cpp b/main/src/vtr_pose_graph/src/serializable/rc_edge.cpp
index 208aeee58..87a36dc11 100644
--- a/main/src/vtr_pose_graph/src/serializable/rc_edge.cpp
+++ b/main/src/vtr_pose_graph/src/serializable/rc_edge.cpp
@@ -68,9 +68,9 @@ TransformMsg toMsg(const TransformT& T) {
} // namespace
RCEdge::RCEdge(const VertexId& from_id, const VertexId& to_id,
- const EdgeType& type, const bool manual,
+ const EdgeType& type, const EdgeMode& mode,
const EdgeTransform& T_to_from)
- : EdgeBase(from_id, to_id, type, manual, T_to_from) {
+ : EdgeBase(from_id, to_id, type, mode, T_to_from) {
const auto data = std::make_shared();
msg_ = std::make_shared>(data);
}
@@ -80,7 +80,10 @@ RCEdge::RCEdge(const EdgeMsg& msg,
: EdgeBase(msg.from_id, msg.to_id,
msg.type.type == EdgeTypeMsg::TEMPORAL ? EdgeType::Temporal
: EdgeType::Spatial,
- msg.mode.mode == EdgeModeMsg::MANUAL, fromMsg(msg.t_to_from)),
+ msg.mode.mode == EdgeModeMsg::MANUAL ? EdgeMode::Manual :
+ msg.mode.mode == EdgeModeMsg::AUTONOMOUS ? EdgeMode::Autonomous
+ : EdgeMode::Unknown,
+ fromMsg(msg.t_to_from)),
msg_(msg_ptr) {}
storage::LockableMessage::Ptr RCEdge::serialize() {
@@ -92,7 +95,7 @@ storage::LockableMessage::Ptr RCEdge::serialize() {
// potentially updated info
const auto type = static_cast(type_);
- const auto mode = manual_ ? EdgeModeMsg::MANUAL : EdgeModeMsg::AUTONOMOUS;
+ const auto mode = static_cast(mode_);
const auto from_id = (uint64_t)from_;
const auto to_id = (uint64_t)to_;
@@ -140,7 +143,7 @@ storage::LockableMessage::Ptr RCEdge::serialize() {
CLOG(DEBUG, "pose_graph") << "Edge " << id_ << " -> ROS msg: "
<< "from: " << from_ << ", to: " << to_
- << ", mode (0:auto, 1:manual): " << manual_
+ << ", mode (0:auto, 1:manual): " << mode
<< ", type (0:temporal, 1:spatial): " << type
<< ", T_to_from: " << T_to_from_.vec().transpose()
<< ", edge changed " << changed;
diff --git a/main/src/vtr_pose_graph/src/serializable/rc_graph.cpp b/main/src/vtr_pose_graph/src/serializable/rc_graph.cpp
index 0a8713a8b..8c2e81904 100644
--- a/main/src/vtr_pose_graph/src/serializable/rc_graph.cpp
+++ b/main/src/vtr_pose_graph/src/serializable/rc_graph.cpp
@@ -34,9 +34,9 @@ RCGraph::RCGraph(const std::string& file_path, const bool load,
fs::path{file_path} / "data", Name2AccessorMapBase())), read_only_(read_only) {
if (load && fs::exists(fs::path(file_path_) / "index")) {
CLOG(INFO, "pose_graph") << "Loading pose graph from " << file_path;
- loadGraphIndex();
loadVertices();
loadEdges();
+ loadGraphIndex();
buildSimpleGraph();
} else {
if (read_only) {
@@ -57,14 +57,47 @@ void RCGraph::save() {
return;
}
CLOG(INFO, "pose_graph") << "Saving pose graph";
- saveGraphIndex();
saveVertices();
- saveEdges();
+ // Flush any edges that saveEdgesLive() held back (always the last edge in the
+ // queue, which is withheld until the next vertex arrives to confirm it is complete).
+ if (!edges_to_write_.empty()) {
+ EdgeMsgAccessor accessor{fs::path{file_path_}, "edges", "vtr_pose_graph_msgs/msg/Edge"};
+ while (!edges_to_write_.empty()) {
+ accessor.write(edges_to_write_.front()->serialize());
+ edges_to_write_.pop();
+ }
+ } else {
+ saveEdges();
+ }
CLOG(INFO, "pose_graph") << "Saving pose graph - DONE!";
}
+void RCGraph::saveLive() {
+ if(read_only_) {
+ CLOG(ERROR, "pose_graph") << "Tried to write to a read only graph!";
+ return;
+ }
+ CLOG(INFO, "pose_graph") << "Saving live pose graph";
+ // saveGraphIndex();
+ saveVerticesLive();
+ saveEdgesLive();
+ CLOG(INFO, "pose_graph") << "Saving live pose graph - DONE!";
+}
+
auto RCGraph::addVertex(const Timestamp& time) -> VertexPtr {
- return GraphType::addVertex(time, name2accessor_map_);
+ // OLD: return GraphType::addVertex(time, name2accessor_map_);
+ auto vertex = GraphType::addVertex(time, name2accessor_map_);
+ vertices_to_write_.push(vertex);
+ if (curr_minor_id_ == 0) saveGraphIndex();
+ return vertex;
+}
+
+auto RCGraph::addEdge(const VertexId& from, const VertexId& to,
+ const EdgeType& type, const EdgeMode& mode,
+ const EdgeTransform& T_to_from) -> EdgePtr {
+ auto edge = GraphType::addEdge(from, to, type, mode, T_to_from); // call base
+ edges_to_write_.push(edge);
+ return edge;
}
void RCGraph::loadGraphIndex() {
@@ -72,9 +105,10 @@ void RCGraph::loadGraphIndex() {
msg_ = accessor.readAtIndex(1);
if (!msg_) {
std::string err{"Graph index message does not exist."};
- CLOG(ERROR, "pose_graph") << err;
- throw std::runtime_error{err};
- }
+ return;
+ // CLOG(ERROR, "pose_graph") << err;
+ // throw std::runtime_error{err};
+ }
const auto data = msg_->locked().get().getData();
CLOG(DEBUG, "pose_graph") << "Loading pose graph index from disk";
@@ -92,12 +126,20 @@ void RCGraph::loadVertices() {
CLOG(DEBUG, "pose_graph") << "Loading vertices from disk";
VertexMsgAccessor accessor{fs::path{file_path_}, "vertices", "vtr_pose_graph_msgs/msg/Vertex", read_only_};
- for (int index = 1;; index++) {
+ int index = 1;
+ for (;; index++) {
const auto msg = accessor.readAtIndex(index);
- if (!msg) break;
-
+ if (!msg) {
+ CLOG(DEBUG, "pose_graph") << "loadVertices: !msg break";
+ break;
+ }
+ lastVertexIdx_ = index;
auto vertex_msg = msg->locked().get().getData();
auto vertex = RCVertex::MakeShared(vertex_msg, name2accessor_map_, msg);
+ if (vertex->vertexTime() == 0) {
+ topology_vertices_[vertex->id()] = index;
+ CLOG(DEBUG, "pose_graph") << "Live: Topology Vertex inserted, len " << topology_vertices_.size();
+ }
vertices_.insert(std::make_pair(vertex->id(), vertex));
CLOG(DEBUG, "pose_graph") << "- loaded vertex " << *vertex;
}
@@ -107,17 +149,206 @@ void RCGraph::loadEdges() {
CLOG(DEBUG, "pose_graph") << "Loading edges from disk";
EdgeMsgAccessor accessor{fs::path{file_path_}, "edges", "vtr_pose_graph_msgs/msg/Edge", read_only_};
- for (int index = 1;; index++) {
+ int index = 1;
+ for (;; index++) {
const auto msg = accessor.readAtIndex(index);
- if (!msg) break;
-
+ if (!msg) {
+ CLOG(DEBUG, "pose_graph") << "loadEdges: !msg break";
+ break;
+ }
auto edge_msg = msg->locked().get().getData();
auto edge = RCEdge::MakeShared(edge_msg, msg);
- edges_.insert(std::make_pair(edge->id(), edge));
+ const auto& eid = edge->id();
+ if (vertices_.find(eid.id1()) == vertices_.end() ||
+ vertices_.find(eid.id2()) == vertices_.end()) {
+ CLOG(WARNING, "pose_graph") << "Skipping dangling edge " << eid;
+ break;
+ }
+ lastEdgeIdx_ = index;
+ if (edge_msg.mode.mode == vtr_pose_graph_msgs::msg::EdgeMode::UNKNOWN) {
+ topology_edges_[edge->id()] = index;
+ CLOG(DEBUG, "pose_graph") << "Topology Edge inserted, len " << topology_edges_.size();
+ }
+ edges_.insert(std::make_pair(eid, edge));
CLOG(DEBUG, "pose_graph") << " - loaded edge " << *edge;
}
}
+void RCGraph::loadVerticesLive() {
+ CLOG(DEBUG, "pose_graph") << "Live Loading vertices from disk | index: " << lastVertexIdx_;
+ VertexMsgAccessor accessor{fs::path{file_path_}, "vertices", "vtr_pose_graph_msgs/msg/Vertex", true};
+
+ std::vector vertices_to_announce;
+ {
+ std::unique_lock lock(mutex_);
+ int index = lastVertexIdx_;
+ for (;; index++) {
+ const auto msg = accessor.readAtIndex(index);
+ if (!msg) {
+ CLOG(DEBUG, "pose_graph") << "loadVerticesLive: !msg break";
+ break;
+ }
+ CLOG(DEBUG, "pose_graph") << "lastVertexIdx_: " << lastVertexIdx_;
+
+ auto vertex_msg = msg->locked().get().getData();
+ auto vertex = RCVertex::MakeShared(vertex_msg, name2accessor_map_, msg);
+ CLOG(DEBUG, "pose_graph") << "loadVerticesLive: (vertex, time): (" << vertex->id() << ", " << vertex->vertexTime() << ")";
+
+ // check if this is an echo (we already have this data)
+ if (vertices_.find(vertex->id()) != vertices_.end()) {
+ continue; // We already wrote this, ignore the echo
+ }
+ lastVertexIdx_ = index;
+
+ // check if timestamp is popualated (if not -> topology)
+ if (vertex->vertexTime() == 0) {
+ topology_vertices_[vertex->id()] = index;
+ CLOG(DEBUG, "pose_graph") << "Live: Topology Vertex inserted, len " << topology_vertices_.size();
+ }
+ vertices_.insert(std::make_pair(vertex->id(), vertex)); // bookkeeping
+ graph_.addVertex(vertex->id()); // add to simpleGraph
+ vertices_to_announce.push_back(vertex); // store for callback
+ }
+ }
+ for (const auto& vertex : vertices_to_announce) {
+ CLOG(DEBUG, "pose_graph") << "loadVerticesLive: callback for " << vertex->id();
+ callback_->vertexAdded(vertex); // inform graph_map_server
+ }
+}
+
+void RCGraph::loadEdgesLive() {
+ CLOG(DEBUG, "pose_graph") << "Live Loading edges from disk | index: " << lastEdgeIdx_;
+ EdgeMsgAccessor accessor{fs::path{file_path_}, "edges", "vtr_pose_graph_msgs/msg/Edge", true};
+
+ std::vector edges_to_announce;
+ {
+ std::unique_lock lock(mutex_);
+ int index = lastEdgeIdx_;
+ for (;; index++) {
+ const auto msg = accessor.readAtIndex(index);
+ if (!msg) {
+ CLOG(DEBUG, "pose_graph") << "loadEdgesLive: !msg break";
+ break;
+ }
+ CLOG(DEBUG, "pose_graph") << "lastEdgeIdx_: " << lastEdgeIdx_;
+
+ auto edge_msg = msg->locked().get().getData();
+ auto edge = RCEdge::MakeShared(edge_msg, msg);
+ const auto& eid = edge->id();
+
+ // check if we already have this edge
+ if (edges_.find(eid) != edges_.end()) { // does this skip teach edge?
+ continue; // We already wrote this, ignore the echo
+ }
+
+ if (vertices_.find(eid.id1()) == vertices_.end() ||
+ vertices_.find(eid.id2()) == vertices_.end()) {
+ CLOG(WARNING, "pose_graph") << "Skipping dangling edge " << eid;
+ break;
+ }
+ lastEdgeIdx_ = index;
+ if (edge_msg.mode.mode == vtr_pose_graph_msgs::msg::EdgeMode::UNKNOWN) {
+ topology_edges_[edge->id()] = index;
+ CLOG(DEBUG, "pose_graph") << "Live: Topology Edge inserted, len " << topology_edges_.size();
+ }
+ if (edge_msg.mode.mode == vtr_pose_graph_msgs::msg::EdgeMode::AUTONOMOUS) {
+ continue;
+ }
+ CLOG(DEBUG, "pose_graph") << " - loading edge " << *edge;
+ CLOG(DEBUG, "pose_graph") << "loadLive: edges_.insert";
+ edges_.insert(std::make_pair(eid, edge)); // bookkeeping
+ CLOG(DEBUG, "pose_graph") << "loadLive: graph_.addEdge";
+ graph_.addEdge(edge->id()); // add to simpleGraph
+ edges_to_announce.push_back(edge);
+ }
+ }
+
+ CLOG(DEBUG, "pose_graph") << "loadLive: callback_->edgeAdded";
+ for (const auto& edge : edges_to_announce){
+ CLOG(DEBUG, "pose_graph") << "loadEdgesLive: callback for " << edge->id();
+ callback_->edgeAdded(edge); // inform graph_map_server
+ }
+}
+
+void RCGraph::populateLive() {
+ CLOG(DEBUG, "pose_graph") << "Monitoring topology-only edges";
+ CLOG(DEBUG, "pose_graph") << "# topology edges, vertices: " << topology_edges_.size() << ", " << topology_vertices_.size();
+ // Monitor edges that were topology, overwrite internal rep. of edges/vertices
+ EdgeMsgAccessor edge_accessor{fs::path{file_path_}, "edges", "vtr_pose_graph_msgs/msg/Edge", true};
+ VertexMsgAccessor vtx_accessor{fs::path{file_path_}, "vertices", "vtr_pose_graph_msgs/msg/Vertex", true};
+
+ std::map edges_to_publish;
+ {
+ std::unique_lock lock(mutex_);
+
+ // populate edges
+ for (auto e_it = topology_edges_.begin(); e_it != topology_edges_.end();) {
+ const auto msg = edge_accessor.readAtIndex(e_it->second);
+ if (!msg) {
+ CLOG(DEBUG, "pose_graph") << "populateEdgesLive: !msg break";
+ break;
+ }
+ auto updated_edge_msg = msg->locked().get().getData();
+ auto new_edge = RCEdge::MakeShared(updated_edge_msg, msg);
+ const auto& eid = new_edge->id();
+
+ CLOG(DEBUG, "pose_graph") << "populateEdgesLive: considering edge" << *new_edge;
+ if (new_edge->isManual()) {
+ CLOG(DEBUG, "pose_graph") << "populateEdgesLive: overwrote edge" << eid;
+ edges_[eid] = new_edge;
+ edges_to_publish[e_it->second] = new_edge;
+ e_it = topology_edges_.erase(e_it);
+ }
+ else {
+ ++e_it;
+ }
+ }
+ }
+
+ {
+ std::unique_lock lock(mutex_);
+
+ //populate vertices
+ for (auto vtx_it = topology_vertices_.begin(); vtx_it != topology_vertices_.end();) {
+ const auto msg = vtx_accessor.readAtIndex(vtx_it->second);
+ if (!msg) {
+ CLOG(DEBUG, "pose_graph") << "populateVerticesLive: !msg break";
+ break;
+ }
+ auto updated_vtx_msg = msg->locked().get().getData();
+ auto new_vertex = RCVertex::MakeShared(updated_vtx_msg, name2accessor_map_, msg);
+
+ if (new_vertex->vertexTime() != 0 ) {
+ const auto& vid = new_vertex->id();
+ vertices_[vid] = new_vertex;
+ CLOG(DEBUG, "pose_graph") << "populateLive: overwrote vertex " << vid << " with real timestamp: " << new_vertex->vertexTime();
+ vtx_it = topology_vertices_.erase(vtx_it);
+ } else {
+ ++vtx_it;
+ }
+ }
+ }
+
+ // publish UI updates
+ for (const auto& [index, edge] : edges_to_publish) {
+ CLOG(DEBUG, "pose_graph") << "populateLive: publishing " << *edge;
+ callback_->publishUpdate(edge);
+ }
+
+}
+
+void RCGraph::loadLive() {
+ CLOG(DEBUG, "pose_graph") << "loadLive";
+ // guard against new graphs
+ if (fs::exists(fs::path(file_path_) / "vertices/vertices_0.db3")){
+ map_info_ = getMapInfo();
+ loadVerticesLive();
+ loadEdgesLive();
+ populateLive();
+ }
+ return;
+}
+
void RCGraph::buildSimpleGraph() {
// First add all vertices to the simple graph
for (auto it = vertices_.begin(); it != vertices_.end(); ++it)
@@ -146,6 +377,7 @@ void RCGraph::saveGraphIndex() {
msg_->locked().get().setData(data);
GraphMsgAccessor accessor{fs::path{file_path_}, "index", "vtr_pose_graph_msgs/msg/Graph"};
+ CLOG(DEBUG, "pose_graph") << "Saving graph index";
accessor.write(msg_);
}
@@ -164,6 +396,25 @@ void RCGraph::saveVertices() {
accessor.write(it->second->serialize());
}
+void RCGraph::saveVerticesLive() {
+ if(read_only_) {
+ CLOG(ERROR, "pose_graph") << "Tried to write to a read only graph!";
+ return;
+ }
+ // save any unsaved data first
+ CLOG(DEBUG, "pose_graph") << "Saving vertices to disk, Vertex Q len : " << vertices_to_write_.size();
+ {
+ std::unique_lock lock(mutex_);
+ VertexMsgAccessor accessor{fs::path{file_path_}, "vertices", "vtr_pose_graph_msgs/msg/Vertex"};
+ while (vertices_to_write_.size() > 0){
+ auto vertex = vertices_to_write_.front();
+ vertices_to_write_.pop();
+ vertex->unload();
+ accessor.write(vertex->serialize());
+ }
+ }
+}
+
void RCGraph::saveEdges() {
if(read_only_) {
CLOG(ERROR, "pose_graph") << "Tried to write to a read only graph!";
@@ -176,5 +427,25 @@ void RCGraph::saveEdges() {
accessor.write(it->second->serialize());
}
+void RCGraph::saveEdgesLive() {
+ if(read_only_) {
+ CLOG(ERROR, "pose_graph") << "Tried to write to a read only graph!";
+ return;
+ }
+ // save any unsaved data first
+ CLOG(DEBUG, "pose_graph") << "Saving Edges to disk, Edge Q len : " << edges_to_write_.size();
+
+ {
+ std::unique_lock lock(mutex_);
+ EdgeMsgAccessor accessor{fs::path{file_path_}, "edges", "vtr_pose_graph_msgs/msg/Edge"};
+ // save all but current edge
+ while (edges_to_write_.size() > 1){
+ auto edge = edges_to_write_.front();
+ edges_to_write_.pop();
+ accessor.write(edge->serialize());
+ }
+ }
+}
+
} // namespace pose_graph
} // namespace vtr
diff --git a/main/src/vtr_pose_graph/src/simple_graph/simple_graph.cpp b/main/src/vtr_pose_graph/src/simple_graph/simple_graph.cpp
index a307e9fa6..fb3cd8922 100644
--- a/main/src/vtr_pose_graph/src/simple_graph/simple_graph.cpp
+++ b/main/src/vtr_pose_graph/src/simple_graph/simple_graph.cpp
@@ -58,11 +58,12 @@ void SimpleGraph::addEdge(const EdgeId& edge) {
// Get iterators to id-node pairs, or create them
auto node1 = (node_map_.emplace(edge.id1(), SimpleNode(edge.id1()))).first;
auto node2 = (node_map_.emplace(edge.id2(), SimpleNode(edge.id2()))).first;
+ CLOG(WARNING, "pose_graph") << "addEdge: adding " << edge;
// Check that edge does not exist
const std::list& adj = node1->second.getAdjacent();
if (std::find(adj.begin(), adj.end(), edge.id2()) != adj.end()) {
- CLOG(ERROR, "pose_graph") << "Edge " << edge << " already exists";
+ CLOG(WARNING, "pose_graph") << "Edge " << edge << " already exists";
throw std::invalid_argument("Tried to add edge that already exists!");
}
diff --git a/main/src/vtr_pose_graph/test/evaluator/test_evaluator.cpp b/main/src/vtr_pose_graph/test/evaluator/test_evaluator.cpp
index ec2a1c265..268776fa3 100644
--- a/main/src/vtr_pose_graph/test/evaluator/test_evaluator.cpp
+++ b/main/src/vtr_pose_graph/test/evaluator/test_evaluator.cpp
@@ -65,13 +65,13 @@ class EvaluatorTestFixture : public Test {
graph_->addVertex();
graph_->addVertex();
graph_->addVertex();
- graph_->addEdge(VertexId(idx, 0), VertexId(idx, 1), EdgeType::Temporal, false, trivialTransform(VertexId(idx, 0), VertexId(idx, 1)));
- graph_->addEdge(VertexId(idx, 1), VertexId(idx, 2), EdgeType::Temporal, false, trivialTransform(VertexId(idx, 1), VertexId(idx, 2)));
+ graph_->addEdge(VertexId(idx, 0), VertexId(idx, 1), EdgeType::Temporal, EdgeMode::Autonomous, trivialTransform(VertexId(idx, 0), VertexId(idx, 1)));
+ graph_->addEdge(VertexId(idx, 1), VertexId(idx, 2), EdgeType::Temporal, EdgeMode::Autonomous, trivialTransform(VertexId(idx, 1), VertexId(idx, 2)));
}
- graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, false, trivialTransform(VertexId(1, 1), VertexId(0, 0)));
- graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, false, trivialTransform(VertexId(2, 2), VertexId(1, 2)));
- graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, false, trivialTransform(VertexId(3, 1), VertexId(2, 1)));
- graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, false, trivialTransform(VertexId(4, 2), VertexId(3, 2)));
+ graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, EdgeMode::Autonomous, trivialTransform(VertexId(1, 1), VertexId(0, 0)));
+ graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, EdgeMode::Autonomous, trivialTransform(VertexId(2, 2), VertexId(1, 2)));
+ graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, EdgeMode::Autonomous, trivialTransform(VertexId(3, 1), VertexId(2, 1)));
+ graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, EdgeMode::Autonomous, trivialTransform(VertexId(4, 2), VertexId(3, 2)));
// clang-format on
}
diff --git a/main/src/vtr_pose_graph/test/evaluator/test_evaluator_base.cpp b/main/src/vtr_pose_graph/test/evaluator/test_evaluator_base.cpp
index b5577d5f0..5382519e4 100644
--- a/main/src/vtr_pose_graph/test/evaluator/test_evaluator_base.cpp
+++ b/main/src/vtr_pose_graph/test/evaluator/test_evaluator_base.cpp
@@ -66,13 +66,13 @@ class EvaluatorTestFixture : public Test {
graph_->addVertex();
graph_->addVertex();
graph_->addVertex();
- graph_->addEdge(VertexId(idx, 0), VertexId(idx, 1), EdgeType::Temporal, false, EdgeTransform(true));
- graph_->addEdge(VertexId(idx, 1), VertexId(idx, 2), EdgeType::Temporal, false, EdgeTransform(true));
+ graph_->addEdge(VertexId(idx, 0), VertexId(idx, 1), EdgeType::Temporal, EdgeMode::Autonomous, EdgeTransform(true));
+ graph_->addEdge(VertexId(idx, 1), VertexId(idx, 2), EdgeType::Temporal, EdgeMode::Autonomous, EdgeTransform(true));
}
- graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, false, EdgeTransform(true));
- graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, false, EdgeTransform(true));
- graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, false, EdgeTransform(true));
- graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, false, EdgeTransform(true));
+ graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
+ graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
+ graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
+ graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
// clang-format on
}
diff --git a/main/src/vtr_pose_graph/test/evaluator/test_subgraph_evaluator.cpp b/main/src/vtr_pose_graph/test/evaluator/test_subgraph_evaluator.cpp
index 91d3bb735..1e97cc26e 100644
--- a/main/src/vtr_pose_graph/test/evaluator/test_subgraph_evaluator.cpp
+++ b/main/src/vtr_pose_graph/test/evaluator/test_subgraph_evaluator.cpp
@@ -53,14 +53,14 @@ class SubGraphTestFixture : public Test {
graph_->addVertex();
for (int vidx = 1; vidx < 250; ++vidx) {
graph_->addVertex();
- graph_->addEdge(VertexId(idx, vidx - 1), VertexId(idx, vidx), EdgeType::Temporal, (idx == 0 || idx == 2) ? true : false, EdgeTransform(true));
+ graph_->addEdge(VertexId(idx, vidx - 1), VertexId(idx, vidx), EdgeType::Temporal, (idx == 0 || idx == 2) ? EdgeMode::Manual : EdgeMode::Autonomous, EdgeTransform(true));
}
}
// Add spatial edge across runs.
- graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, false, EdgeTransform(true));
- graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, false, EdgeTransform(true));
- graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, false, EdgeTransform(true));
- graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, false, EdgeTransform(true));
+ graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
+ graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
+ graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
+ graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
// clang-format on
}
diff --git a/main/src/vtr_pose_graph/test/index/test_edge_vertex_base.cpp b/main/src/vtr_pose_graph/test/index/test_edge_vertex_base.cpp
index 129df2d9a..15f4d3dfe 100644
--- a/main/src/vtr_pose_graph/test/index/test_edge_vertex_base.cpp
+++ b/main/src/vtr_pose_graph/test/index/test_edge_vertex_base.cpp
@@ -29,7 +29,7 @@ TEST(PoseGraph, edge_base_tests) {
/// constructors
// follow the convention of from a higher run to a lower run, so the edge is
// reversed
- EdgeBase edge(VertexId(3, 5), VertexId(1, 10), EdgeType::Temporal, false,
+ EdgeBase edge(VertexId(3, 5), VertexId(1, 10), EdgeType::Temporal, EdgeMode::Autonomous,
EdgeTransform(true));
edge.setTransform(EdgeTransform(true));
diff --git a/main/src/vtr_pose_graph/test/index/test_graph_structure.cpp b/main/src/vtr_pose_graph/test/index/test_graph_structure.cpp
index 4fad7afb5..5fee4cbc1 100644
--- a/main/src/vtr_pose_graph/test/index/test_graph_structure.cpp
+++ b/main/src/vtr_pose_graph/test/index/test_graph_structure.cpp
@@ -99,7 +99,7 @@ TEST_F(GraphStructureTestFixture, multiple_run_vertex_addition) {
TEST_F(GraphStructureTestFixture, add_edge_to_empty_graph) {
EXPECT_THROW(graph_->addEdge(VertexId(0, 0), VertexId(0, 1),
- EdgeType::Temporal, false, EdgeTransform(true)),
+ EdgeType::Temporal, EdgeMode::Autonomous, EdgeTransform(true)),
std::range_error);
}
@@ -121,7 +121,7 @@ class EdgeTestFixture : public Test {
TEST_F(EdgeTestFixture, iterator_test_bfs_dfs) {
for (int idx = 0; idx < 9; ++idx)
graph_->addEdge(VertexId(0, idx), VertexId(0, idx + 1), EdgeType::Temporal,
- true, EdgeTransform(true));
+ EdgeMode::Manual, EdgeTransform(true));
/// \note Never use iterator of a graph that may be used in a multi-threading
/// context!! Call getsubgraph first to get only portion you need. This is
@@ -150,7 +150,7 @@ TEST_F(EdgeTestFixture, add_temporal_edge) {
VertexId to(0, 1);
auto edge =
- graph_->addEdge(from, to, EdgeType::Temporal, true, EdgeTransform(true));
+ graph_->addEdge(from, to, EdgeType::Temporal, EdgeMode::Manual, EdgeTransform(true));
EXPECT_TRUE(edge != nullptr);
EXPECT_TRUE(edge->isTemporal());
EXPECT_FALSE(edge->isSpatial());
@@ -168,7 +168,7 @@ TEST_F(EdgeTestFixture, add_spatial_edge) {
VertexId to(0, 1);
auto edge =
- graph_->addEdge(from, to, EdgeType::Spatial, false, EdgeTransform(true));
+ graph_->addEdge(from, to, EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
EXPECT_TRUE(edge != nullptr);
EXPECT_FALSE(edge->isTemporal());
EXPECT_TRUE(edge->isSpatial());
@@ -182,7 +182,7 @@ TEST_F(EdgeTestFixture, cross_run_edge) {
VertexId from(1, 6);
auto edge =
- graph_->addEdge(from, to, EdgeType::Spatial, false, EdgeTransform(true));
+ graph_->addEdge(from, to, EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
EXPECT_TRUE(edge != nullptr);
EXPECT_FALSE(edge->isTemporal());
EXPECT_TRUE(edge->isSpatial());
@@ -191,7 +191,7 @@ TEST_F(EdgeTestFixture, cross_run_edge) {
// Cannot add from low runs to high ones
EXPECT_THROW(
- graph_->addEdge(to, from, EdgeType::Spatial, true, EdgeTransform(true)),
+ graph_->addEdge(to, from, EdgeType::Spatial, EdgeMode::Manual, EdgeTransform(true)),
std::invalid_argument);
}
diff --git a/main/src/vtr_pose_graph/test/index/test_subgraph.cpp b/main/src/vtr_pose_graph/test/index/test_subgraph.cpp
index e04566b04..4f676b330 100644
--- a/main/src/vtr_pose_graph/test/index/test_subgraph.cpp
+++ b/main/src/vtr_pose_graph/test/index/test_subgraph.cpp
@@ -55,18 +55,18 @@ class SubGraphTestFixture : public Test {
for (int vidx = 1; vidx < 250; ++vidx) {
graph_->addVertex();
graph_->addEdge(VertexId(idx, vidx - 1), VertexId(idx, vidx),
- EdgeType::Temporal, idx == 0 ? true : false,
+ EdgeType::Temporal, idx == 0 ? EdgeMode::Manual : EdgeMode::Autonomous,
EdgeTransform(true));
}
}
// Add spatial edge across runs.
- graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, false,
+ graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, EdgeMode::Autonomous,
EdgeTransform(true));
- graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, false,
+ graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, EdgeMode::Autonomous,
EdgeTransform(true));
- graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, false,
+ graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, EdgeMode::Autonomous,
EdgeTransform(true));
- graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, false,
+ graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, EdgeMode::Autonomous,
EdgeTransform(true));
// set the edge's transform to something special;
diff --git a/main/src/vtr_pose_graph/test/path/test_localization_chain.cpp b/main/src/vtr_pose_graph/test/path/test_localization_chain.cpp
index 6310ec581..0cfd45b02 100644
--- a/main/src/vtr_pose_graph/test/path/test_localization_chain.cpp
+++ b/main/src/vtr_pose_graph/test/path/test_localization_chain.cpp
@@ -66,7 +66,7 @@ class ChainTest : public Test {
EdgeTransform edge_transform(transform);
edge_transform.setZeroCovariance();
graph_->addEdge(VertexId(0, i), VertexId(0, i + 1), EdgeType::Temporal,
- true, edge_transform);
+ EdgeMode::Manual, edge_transform);
}
// R1
@@ -79,7 +79,7 @@ class ChainTest : public Test {
EdgeTransform edge_transform(transform);
edge_transform.setZeroCovariance();
graph_->addEdge(VertexId(1, i), VertexId(1, i + 1), EdgeType::Temporal,
- false, edge_transform);
+ EdgeMode::Autonomous, edge_transform);
}
using PrivEvaluator = eval::mask::privileged::CachedEval;
diff --git a/main/src/vtr_pose_graph/test/path/test_path.cpp b/main/src/vtr_pose_graph/test/path/test_path.cpp
index e8f8aaeb3..d4f1567cf 100644
--- a/main/src/vtr_pose_graph/test/path/test_path.cpp
+++ b/main/src/vtr_pose_graph/test/path/test_path.cpp
@@ -49,11 +49,11 @@ class PathTest : public Test {
graph_->addVertex();
for (int minor_idx = 0; minor_idx < 5 - 1; ++minor_idx) {
graph_->addVertex();
- graph_->addEdge(VertexId(major_idx, minor_idx), VertexId(major_idx, minor_idx + 1), EdgeType::Temporal, false, EdgeTransform(true));
+ graph_->addEdge(VertexId(major_idx, minor_idx), VertexId(major_idx, minor_idx + 1), EdgeType::Temporal, EdgeMode::Autonomous, EdgeTransform(true));
}
}
// Add spatial edge across runs.
- graph_->addEdge(VertexId(1, 2), VertexId(0, 1), EdgeType::Spatial, false, EdgeTransform(true));
+ graph_->addEdge(VertexId(1, 2), VertexId(0, 1), EdgeType::Spatial, EdgeMode::Autonomous, EdgeTransform(true));
// set the edge's transform to something special;
for (auto itr = graph_->beginEdge(); itr != graph_->endEdge(); ++itr) {
@@ -90,11 +90,11 @@ class MergedPathTest : public Test {
graph_->addVertex();
for (int minor_idx = 0; minor_idx < 4 - 1; ++minor_idx) {
graph_->addVertex();
- graph_->addEdge(VertexId(major_idx, minor_idx), VertexId(major_idx, minor_idx + 1), EdgeType::Temporal, false, EdgeTransform(true));
+ graph_->addEdge(VertexId(major_idx, minor_idx), VertexId(major_idx, minor_idx + 1), EdgeType::Temporal, EdgeMode::Autonomous, EdgeTransform(true));
}
}
// Add merged loop closure edge across runs.
- graph_->addEdge(VertexId(0, 3), VertexId(0, 0), EdgeType::Temporal, false, EdgeTransform(true));
+ graph_->addEdge(VertexId(0, 3), VertexId(0, 0), EdgeType::Temporal, EdgeMode::Autonomous, EdgeTransform(true));
// set the edge's transform to something special;
diff --git a/main/src/vtr_pose_graph/test/serializable/test_serialization_edge.cpp b/main/src/vtr_pose_graph/test/serializable/test_serialization_edge.cpp
index 0bf71728b..e10875133 100644
--- a/main/src/vtr_pose_graph/test/serializable/test_serialization_edge.cpp
+++ b/main/src/vtr_pose_graph/test/serializable/test_serialization_edge.cpp
@@ -22,6 +22,10 @@
#include "std_msgs/msg/string.hpp"
+#include // uuid class
+#include // generators
+#include // streaming operators etc.
+
#include "vtr_logging/logging_init.hpp"
#include "vtr_pose_graph/serializable/rc_edge.hpp"
@@ -31,6 +35,14 @@ using namespace vtr::pose_graph;
using namespace vtr::storage;
using StringMsg = std_msgs::msg::String;
+using Id = boost::uuids::uuid;
+
+static boost::uuids::random_generator gen;
+const uint64_t ID0 = *reinterpret_cast(gen().data);
+const uint64_t ID1 = *reinterpret_cast(gen().data);
+const uint64_t ID2 = *reinterpret_cast(gen().data);
+const uint64_t ID3 = *reinterpret_cast(gen().data);
+const uint64_t ID4 = *reinterpret_cast(gen().data);
namespace {
@@ -64,7 +76,7 @@ TransformMsg toMsg(const TransformT& T) {
} // namespace
TEST(TestSerializationEdge, construct_spatial_edge_directly) {
- VertexId to(0, 1), from(1, 3);
+ VertexId to(ID0, ID1), from(ID1, ID3);
using TransformVecT = Eigen::Matrix;
TransformVecT transform_vec;
@@ -72,7 +84,7 @@ TEST(TestSerializationEdge, construct_spatial_edge_directly) {
EdgeTransform transform(transform_vec);
transform.setCovariance(Eigen::Matrix::Identity());
- RCEdge edge(from, to, EdgeType::Spatial, false, transform);
+ RCEdge edge(from, to, EdgeType::Spatial, EdgeMode::Autonomous, transform);
// get the ROS message for the first time
{
@@ -121,7 +133,7 @@ TEST(TestSerializationEdge, construct_spatial_edge_directly) {
}
TEST(TestSerializationEdge, simulate_load_spatial_edge_from_disk) {
- VertexId to(0, 1), from(1, 3);
+ VertexId to(ID0, ID1), from(ID1, ID3);
using TransformVecT = Eigen::Matrix;
TransformVecT transform_vec;
@@ -165,7 +177,7 @@ TEST(TestSerializationEdge, simulate_load_spatial_edge_from_disk) {
}
TEST(TestSerializationEdge, construct_temporal_edge_directly) {
- VertexId from(3, 3), to(3, 4);
+ VertexId from(ID3, ID3), to(ID3, ID4);
using TransformVecT = Eigen::Matrix;
TransformVecT transform_vec;
@@ -173,7 +185,7 @@ TEST(TestSerializationEdge, construct_temporal_edge_directly) {
EdgeTransform transform(transform_vec);
transform.setCovariance(Eigen::Matrix::Identity());
- RCEdge edge(from, to, EdgeType::Temporal, true, transform);
+ RCEdge edge(from, to, EdgeType::Temporal, EdgeMode::Manual, transform);
// get the ROS message for the first time
{
@@ -222,7 +234,7 @@ TEST(TestSerializationEdge, construct_temporal_edge_directly) {
}
TEST(TestSerializationEdge, simulate_load_temporal_edge_from_disk) {
- VertexId from(3, 3), to(3, 4);
+ VertexId from(ID3, ID3), to(ID3, ID4);
using TransformVecT = Eigen::Matrix;
TransformVecT transform_vec;
@@ -266,7 +278,7 @@ TEST(TestSerializationEdge, simulate_load_temporal_edge_from_disk) {
}
TEST(TestSerializationEdge, change_edge_transform) {
- VertexId from(3, 3), to(3, 4);
+ VertexId from(ID3, ID3), to(ID3, ID4);
using TransformVecT = Eigen::Matrix;
TransformVecT transform_vec;
@@ -274,7 +286,7 @@ TEST(TestSerializationEdge, change_edge_transform) {
EdgeTransform transform(transform_vec);
transform.setCovariance(Eigen::Matrix::Identity());
- RCEdge edge(from, to, EdgeType::Temporal, true, transform);
+ RCEdge edge(from, to, EdgeType::Temporal, EdgeMode::Manual, transform);
// get the ROS message for the first time
{
diff --git a/main/src/vtr_pose_graph/test/serializable/test_serialization_graph.cpp b/main/src/vtr_pose_graph/test/serializable/test_serialization_graph.cpp
index 2cc1b1e3e..190be00cd 100644
--- a/main/src/vtr_pose_graph/test/serializable/test_serialization_graph.cpp
+++ b/main/src/vtr_pose_graph/test/serializable/test_serialization_graph.cpp
@@ -13,7 +13,7 @@
// limitations under the License.
/**
- * \file test_serialization_run.cpp
+ * \file test_serialization_graph.cpp
* \author Yuchen Wu, Autonomous Space Robotics Lab (ASRL)
*/
#include
@@ -59,8 +59,6 @@ class TemporaryDirectoryFixture : public Test {
public:
TemporaryDirectoryFixture() {
temp_dir_ = rcpputils::fs::create_temp_directory("tmp_test_dir").string();
- // temp_dir_ = "/home/yuchen/ASRL/temp/test_pose_graph";
- // rcpputils::fs::create_directories(temp_dir_);
graph_dir_ = (rcpputils::fs::path(temp_dir_) / "graph").string();
}
@@ -73,26 +71,19 @@ class TemporaryDirectoryFixture : public Test {
};
TEST_F(TemporaryDirectoryFixture, construct_empty_graph_and_save) {
- // constructor initializes the ros message
RCGraph graph(graph_dir_, false);
- // destructor saves the graph
}
TEST_F(TemporaryDirectoryFixture, simulate_loading_graph_from_disk) {
- // constructor initializes the message
auto graph = std::make_shared(graph_dir_, false);
RCGraph::MapInfoMsg map_info;
map_info.set = true;
- graph->setMapInfo(map_info); // this sets map info (map_info.set=true)
+ graph->setMapInfo(map_info);
- // destructor saves the graph
graph.reset();
- // constructor loads graph
graph = std::make_shared(graph_dir_);
-
- // destructor saves the graph
}
class GraphSerializationFixture : public TemporaryDirectoryFixture {
@@ -114,81 +105,74 @@ class GraphSerializationFixture : public TemporaryDirectoryFixture {
GraphSerializationFixture() {
graph_ = std::make_shared(graph_dir_, false);
// clang-format off
- for (int idx = 0; idx < 5; ++idx) {
+ for (int i = 0; i < 5; ++i) {
graph_->addRun();
- graph_->addVertex(time_stamp_++);
- graph_->addVertex(time_stamp_++);
- graph_->addVertex(time_stamp_++);
- graph_->addEdge(VertexId(idx, 0), VertexId(idx, 1), EdgeType::Temporal, false, trivialTransform(VertexId(idx, 0), VertexId(idx, 1)));
- graph_->addEdge(VertexId(idx, 1), VertexId(idx, 2), EdgeType::Temporal, false, trivialTransform(VertexId(idx, 1), VertexId(idx, 2)));
+ vids_[i][0] = graph_->addVertex(time_stamp_++)->id();
+ vids_[i][1] = graph_->addVertex(time_stamp_++)->id();
+ vids_[i][2] = graph_->addVertex(time_stamp_++)->id();
+ graph_->addEdge(vids_[i][0], vids_[i][1], EdgeType::Temporal, EdgeMode::Autonomous, trivialTransform(vids_[i][0], vids_[i][1]));
+ graph_->addEdge(vids_[i][1], vids_[i][2], EdgeType::Temporal, EdgeMode::Autonomous, trivialTransform(vids_[i][1], vids_[i][2]));
}
- graph_->addEdge(VertexId(1, 1), VertexId(0, 0), EdgeType::Spatial, false, trivialTransform(VertexId(1, 1), VertexId(0, 0)));
- graph_->addEdge(VertexId(2, 2), VertexId(1, 2), EdgeType::Spatial, false, trivialTransform(VertexId(2, 2), VertexId(1, 2)));
- graph_->addEdge(VertexId(3, 1), VertexId(2, 1), EdgeType::Spatial, false, trivialTransform(VertexId(3, 1), VertexId(2, 1)));
- graph_->addEdge(VertexId(4, 2), VertexId(3, 2), EdgeType::Spatial, false, trivialTransform(VertexId(4, 2), VertexId(3, 2)));
+ graph_->addEdge(vids_[1][1], vids_[0][0], EdgeType::Spatial, EdgeMode::Autonomous, trivialTransform(vids_[1][1], vids_[0][0]));
+ graph_->addEdge(vids_[2][2], vids_[1][2], EdgeType::Spatial, EdgeMode::Autonomous, trivialTransform(vids_[2][2], vids_[1][2]));
+ graph_->addEdge(vids_[3][1], vids_[2][1], EdgeType::Spatial, EdgeMode::Autonomous, trivialTransform(vids_[3][1], vids_[2][1]));
+ graph_->addEdge(vids_[4][2], vids_[3][2], EdgeType::Spatial, EdgeMode::Autonomous, trivialTransform(vids_[4][2], vids_[3][2]));
// clang-format on
}
+ void verifyGraphStructure(RCGraph& graph) {
+ for (int i = 0; i < 5; ++i)
+ for (int j = 0; j < 3; ++j)
+ EXPECT_NO_THROW(graph.at(vids_[i][j]));
+
+ RCEdge::Ptr edge;
+ for (int i = 0; i < 5; ++i) {
+ edge = graph.at(EdgeId(vids_[i][0], vids_[i][1]));
+ verifyTransform(vids_[i][0], vids_[i][1], edge->T());
+ edge = graph.at(EdgeId(vids_[i][1], vids_[i][2]));
+ verifyTransform(vids_[i][1], vids_[i][2], edge->T());
+ }
+ edge = graph.at(EdgeId(vids_[1][1], vids_[0][0]));
+ verifyTransform(vids_[1][1], vids_[0][0], edge->T());
+ edge = graph.at(EdgeId(vids_[2][2], vids_[1][2]));
+ verifyTransform(vids_[2][2], vids_[1][2], edge->T());
+ edge = graph.at(EdgeId(vids_[3][1], vids_[2][1]));
+ verifyTransform(vids_[3][1], vids_[2][1], edge->T());
+ edge = graph.at(EdgeId(vids_[4][2], vids_[3][2]));
+ verifyTransform(vids_[4][2], vids_[3][2], edge->T());
+
+ EXPECT_EQ(graph.numberOfEdges(), (unsigned)14);
+ EXPECT_EQ(graph.numberOfVertices(), (unsigned)15);
+ }
+
public:
std::shared_ptr graph_;
Timestamp time_stamp_ = 0;
+ VertexId vids_[5][3];
};
-void verifyGraphStructure(RCGraph& graph) {
- for (int run_idx = 0; run_idx < 5; run_idx++)
- for (int vertex_idx = 0; vertex_idx < 3; vertex_idx++) {
- EXPECT_NO_THROW(graph.at(VertexId(run_idx, vertex_idx)));
- }
-
- RCEdge::Ptr edge;
- for (int idx = 0; idx < 5; ++idx) {
- edge = graph.at(EdgeId(VertexId(idx, 0), VertexId(idx, 1)));
- verifyTransform(VertexId(idx, 0), VertexId(idx, 1), edge->T());
- edge = graph.at(EdgeId(VertexId(idx, 1), VertexId(idx, 2)));
- verifyTransform(VertexId(idx, 1), VertexId(idx, 2), edge->T());
- }
- edge = graph.at(EdgeId(VertexId(1, 1), VertexId(0, 0)));
- verifyTransform(VertexId(1, 1), VertexId(0, 0), edge->T());
- edge = graph.at(EdgeId(VertexId(2, 2), VertexId(1, 2)));
- verifyTransform(VertexId(2, 2), VertexId(1, 2), edge->T());
- edge = graph.at(EdgeId(VertexId(3, 1), VertexId(2, 1)));
- verifyTransform(VertexId(3, 1), VertexId(2, 1), edge->T());
- edge = graph.at(EdgeId(VertexId(4, 2), VertexId(3, 2)));
- verifyTransform(VertexId(4, 2), VertexId(3, 2), edge->T());
-
- EXPECT_EQ(graph.numberOfEdges(), (unsigned)14);
- EXPECT_EQ(graph.numberOfVertices(), (unsigned)15);
-}
-
TEST_F(GraphSerializationFixture, SaveLoadSaveLoad) {
verifyGraphStructure(*graph_);
- // save graph to file
graph_.reset();
- // load again
auto graph = std::make_shared(graph_dir_);
verifyGraphStructure(*graph);
- // save again
graph.reset();
- // load again
graph = std::make_shared(graph_dir_);
verifyGraphStructure(*graph);
}
TEST_F(GraphSerializationFixture, PostLoadSubGraphExtraction) {
- // save graph to file
graph_.reset();
- // load again
auto graph = std::make_shared(graph_dir_);
verifyGraphStructure(*graph);
- VertexId root_id(0, 0);
EXPECT_NO_THROW(graph->getSubgraph(
- root_id, std::make_shared(true, true)));
+ vids_[0][0], std::make_shared(true, true)));
int count = 0;
for (auto it = graph->beginVertex(); it != graph->endVertex(); ++it) count++;
@@ -196,29 +180,25 @@ TEST_F(GraphSerializationFixture, PostLoadSubGraphExtraction) {
}
TEST_F(GraphSerializationFixture, SaveLoadModifySaveLoad) {
- // save graph to file
graph_.reset();
- // load, modify and then save the graph
auto graph = std::make_shared(graph_dir_);
graph->addRun();
- graph->addVertex(time_stamp_++);
- graph->addVertex(time_stamp_++);
- graph->addVertex(time_stamp_++);
- graph->addEdge(VertexId(5, 2), VertexId(0, 2), EdgeType::Spatial, false,
+ auto v0 = graph->addVertex(time_stamp_++);
+ auto v1 = graph->addVertex(time_stamp_++);
+ auto v2 = graph->addVertex(time_stamp_++);
+ graph->addEdge(v2->id(), vids_[0][2], EdgeType::Spatial, EdgeMode::Autonomous,
EdgeTransform(true));
graph.reset();
- // load the graph again
graph = std::make_shared(graph_dir_);
EXPECT_EQ(graph->numberOfVertices(), (unsigned)18);
- EXPECT_NO_THROW(graph->at(EdgeId(VertexId(5, 2), VertexId(0, 2))));
+ EXPECT_NO_THROW(graph->at(EdgeId(v2->id(), vids_[0][2])));
graph.reset();
- // load the graph again
graph = std::make_shared(graph_dir_);
EXPECT_EQ(graph->numberOfVertices(), (unsigned)18);
- EXPECT_NO_THROW(graph->at(EdgeId(VertexId(5, 2), VertexId(0, 2))));
+ EXPECT_NO_THROW(graph->at(EdgeId(v2->id(), vids_[0][2])));
graph.reset();
}
diff --git a/main/src/vtr_pose_graph/test/serializable/test_serialization_vertex.cpp b/main/src/vtr_pose_graph/test/serializable/test_serialization_vertex.cpp
index ecbd4f428..7b5547edd 100644
--- a/main/src/vtr_pose_graph/test/serializable/test_serialization_vertex.cpp
+++ b/main/src/vtr_pose_graph/test/serializable/test_serialization_vertex.cpp
@@ -23,6 +23,10 @@
#include "vtr_logging/logging_init.hpp"
#include "vtr_pose_graph/serializable/rc_vertex.hpp"
+#include // uuid class
+#include // generators
+#include // streaming operators etc.
+
#include "std_msgs/msg/string.hpp"
using namespace ::testing; // NOLINT
@@ -31,9 +35,16 @@ using namespace vtr::pose_graph;
using namespace vtr::storage;
using StringMsg = std_msgs::msg::String;
+using Id = boost::uuids::uuid;
+
+static boost::uuids::random_generator gen;
+const uint64_t ID2 = *reinterpret_cast(gen().data);
+const uint64_t ID6 = *reinterpret_cast(gen().data);
+const uint64_t ID16 = *reinterpret_cast(gen().data);
+const uint64_t ID200 = *reinterpret_cast(gen().data);
TEST(TestSerializationVertex, construct_vertex_directly) {
- VertexId id(2, 6);
+ VertexId id(ID2, ID6);
Timestamp vertex_time(666);
const auto name2accessor_map =
std::make_shared();
@@ -77,7 +88,7 @@ TEST(TestSerializationVertex, construct_vertex_directly) {
}
TEST(TestSerializationVertex, simulate_load_vertex_from_disk) {
- VertexId id(16, 200);
+ VertexId id(ID16, ID200);
Timestamp vertex_time(666);
const auto name2accessor_map =
std::make_shared();
diff --git a/main/src/vtr_pose_graph/test/stream/test_data_read_write.cpp b/main/src/vtr_pose_graph/test/stream/test_data_read_write.cpp
index b9d190f44..3f8a1ce7b 100644
--- a/main/src/vtr_pose_graph/test/stream/test_data_read_write.cpp
+++ b/main/src/vtr_pose_graph/test/stream/test_data_read_write.cpp
@@ -59,7 +59,7 @@ TEST(PoseGraph, ReadWrite) {
for (int idx = 1; idx < VERTICES; ++idx) {
graph->addVertex(stamp++);
graph->addEdge(VertexId(0, idx - 1), VertexId(0, idx), EdgeType::Temporal,
- false, EdgeTransform(true));
+ EdgeMode::Autonomous, EdgeTransform(true));
}
// Generate random data
diff --git a/main/src/vtr_route_planning/include/vtr_route_planning/bfs_planner.hpp b/main/src/vtr_route_planning/include/vtr_route_planning/bfs_planner.hpp
index 722f0d029..d47071cdc 100644
--- a/main/src/vtr_route_planning/include/vtr_route_planning/bfs_planner.hpp
+++ b/main/src/vtr_route_planning/include/vtr_route_planning/bfs_planner.hpp
@@ -42,6 +42,7 @@ class BFSPlanner : public RoutePlannerInterface {
GraphPtr getGraph() const;
/** \brief Returns a privileged graph (only contains teach routes) */
GraphBasePtr getPrivilegedGraph() const;
+ GraphBasePtr getTopologyGraph() const;
/** \brief Computes path from -> to given the privileged graph */
PathType path(const GraphBasePtr &priv_graph, const VertexId &from,
const VertexId &to);
diff --git a/main/src/vtr_route_planning/src/bfs_planner.cpp b/main/src/vtr_route_planning/src/bfs_planner.cpp
index e774d64c5..c62ff721e 100644
--- a/main/src/vtr_route_planning/src/bfs_planner.cpp
+++ b/main/src/vtr_route_planning/src/bfs_planner.cpp
@@ -30,7 +30,8 @@ auto BFSPlanner::path(const VertexId &from, const VertexId::List &to,
}
idx.clear();
- const auto priv_graph = getPrivilegedGraph();
+ // const auto priv_graph = getPrivilegedGraph();
+ const auto priv_graph = getTopologyGraph();
auto rval = path(priv_graph, from, to.front());
idx.push_back(rval.empty() ? 0 : (rval.size() - 1));
@@ -49,7 +50,8 @@ auto BFSPlanner::path(const VertexId &from, const VertexId::List &to,
}
auto BFSPlanner::path(const VertexId &from, const VertexId &to) -> PathType {
- return path(getPrivilegedGraph(), from, to);
+ // return path(getPrivilegedGraph(), from, to);
+ return path(getTopologyGraph(), from, to);
}
auto BFSPlanner::getGraph() const -> GraphPtr {
@@ -68,7 +70,17 @@ auto BFSPlanner::getPrivilegedGraph() const -> GraphBasePtr {
const auto graph = getGraph();
using PrivEval = tactic::PrivilegedEvaluator;
auto priv_eval = std::make_shared(*graph);
- return graph->getSubgraph(priv_eval);
+ const auto root_vid = getGraph()->root();
+ return graph->getSubgraph(root_vid, priv_eval);
+}
+
+auto BFSPlanner::getTopologyGraph() const -> GraphBasePtr {
+ // get the current privileged graph
+ const auto graph = getGraph();
+ using TopEval = tactic::TopologyEvaluator;
+ auto top_eval = std::make_shared(*graph);
+ const auto root_vid = getGraph()->root();
+ return graph->getSubgraph(root_vid, top_eval);
}
auto BFSPlanner::path(const GraphBasePtr &priv_graph, const VertexId &from,
diff --git a/main/src/vtr_storage/include/vtr_storage/accessor/base_reader_interface.hpp b/main/src/vtr_storage/include/vtr_storage/accessor/base_reader_interface.hpp
index 6250ddc0d..46e5130ec 100644
--- a/main/src/vtr_storage/include/vtr_storage/accessor/base_reader_interface.hpp
+++ b/main/src/vtr_storage/include/vtr_storage/accessor/base_reader_interface.hpp
@@ -56,7 +56,7 @@ class BaseReaderInterface {
virtual ~BaseReaderInterface() {}
virtual void open(const std::string & uri, const bool read_only) = 0;
- virtual void close() = 0;
+ virtual void close(const bool read_only) = 0;
virtual std::shared_ptr read_at_timestamp(const Timestamp & timestamp) = 0;
virtual std::vector> read_at_timestamp_range(const Timestamp & timestamp_begin, const Timestamp & timestamp_end) = 0;
diff --git a/main/src/vtr_storage/include/vtr_storage/accessor/base_writer_interface.hpp b/main/src/vtr_storage/include/vtr_storage/accessor/base_writer_interface.hpp
index 66d9e9856..e2c55638f 100644
--- a/main/src/vtr_storage/include/vtr_storage/accessor/base_writer_interface.hpp
+++ b/main/src/vtr_storage/include/vtr_storage/accessor/base_writer_interface.hpp
@@ -51,7 +51,7 @@ class BaseWriterInterface
virtual ~BaseWriterInterface() {}
virtual void open(const std::string & uri, const bool read_only) = 0;
- virtual void close() = 0;
+ virtual void close(const bool read_only) = 0;
virtual void write(const std::shared_ptr & message) = 0;
virtual void write(const std::vector> & messages) = 0;
diff --git a/main/src/vtr_storage/include/vtr_storage/accessor/storage_accessor.hpp b/main/src/vtr_storage/include/vtr_storage/accessor/storage_accessor.hpp
index 55f327d56..4b258af6c 100644
--- a/main/src/vtr_storage/include/vtr_storage/accessor/storage_accessor.hpp
+++ b/main/src/vtr_storage/include/vtr_storage/accessor/storage_accessor.hpp
@@ -44,7 +44,7 @@ class StorageAccessor : public BaseReaderInterface, public BaseWriterInterface {
~StorageAccessor() override;
void open(const std::string &uri, const bool read_only = false) override;
- void close() override;
+ void close(const bool read_only = false) override;
/// Reader
std::shared_ptr read_at_timestamp(const Timestamp & timestamp) override;
diff --git a/main/src/vtr_storage/include/vtr_storage/stream/data_bubble.hpp b/main/src/vtr_storage/include/vtr_storage/stream/data_bubble.hpp
index 17bef0e34..bdc7296f6 100644
--- a/main/src/vtr_storage/include/vtr_storage/stream/data_bubble.hpp
+++ b/main/src/vtr_storage/include/vtr_storage/stream/data_bubble.hpp
@@ -250,7 +250,6 @@ bool DataBubble::unload(bool clear) {
[&](std::pair& element) {
messages.push_back(element.second);
});
-
accessor->write(messages);
}
@@ -292,10 +291,10 @@ auto DataBubble::retrieve(const Timestamp& time) -> MessagePtr {
const LockGuard lock(mutex_);
if (!loaded(time) && !load(time)) {
CLOG(WARNING, "storage")
- << "Message with time stamp " << time
- << " does not exist in cache or disk. Return a nullptr.";
+ << "Message of type [" << typeid(DataType).name() << "] with time stamp "
+ << time << " does not exist in cache or disk. Return a nullptr.";
return nullptr;
- }
+ }
return time2message_map_.at(time);
}
diff --git a/main/src/vtr_storage/src/accessor/storage_accessor.cpp b/main/src/vtr_storage/src/accessor/storage_accessor.cpp
index 44c17a2cc..796430b17 100644
--- a/main/src/vtr_storage/src/accessor/storage_accessor.cpp
+++ b/main/src/vtr_storage/src/accessor/storage_accessor.cpp
@@ -68,33 +68,35 @@ void StorageAccessor::open(const std::string& uri, const bool read_only) {
if (!metadata_io_->metadata_file_exists(uri)) return;
- /// Handle case where metadata exists, meaning there's already a bag
- const auto metadata = metadata_io_->read_metadata(uri);
-
- // sanity check
- rcpputils::check_true(metadata.relative_file_paths.size() == 1,
- "VTR storage does not support multiple bag files.");
- rcpputils::check_true(metadata.relative_file_paths[0] == relative_file_path,
- "Inconsistent database path name detected.");
-
- for (const auto& topic_info : metadata.topics_with_message_count) {
- const auto insert_res = topics_names_to_info_.insert(
- std::make_pair(topic_info.topic_metadata.name, topic_info));
- if (!insert_res.second) {
- std::stringstream errmsg;
- errmsg << "Failed to insert topic \"" << topic_info.topic_metadata.name
- << "\"!";
- throw std::runtime_error(errmsg.str());
+ if (!read_only){
+ /// Handle case where metadata exists, meaning there's already a bag
+ const auto metadata = metadata_io_->read_metadata(uri);
+
+ // sanity check
+ rcpputils::check_true(metadata.relative_file_paths.size() == 1,
+ "VTR storage does not support multiple bag files.");
+ rcpputils::check_true(metadata.relative_file_paths[0] == relative_file_path,
+ "Inconsistent database path name detected.");
+
+ for (const auto& topic_info : metadata.topics_with_message_count) {
+ const auto insert_res = topics_names_to_info_.insert(
+ std::make_pair(topic_info.topic_metadata.name, topic_info));
+ if (!insert_res.second) {
+ std::stringstream errmsg;
+ errmsg << "Failed to insert topic \"" << topic_info.topic_metadata.name
+ << "\"!";
+ throw std::runtime_error(errmsg.str());
+ }
}
}
}
-void StorageAccessor::close() {
+void StorageAccessor::close(const bool read_only) {
std::lock_guard storage_lock(storage_mutex_);
if (!storage_) return;
- if (!base_folder_.empty()) {
+ if (!base_folder_.empty() && !read_only) {
auto metadata = storage_->get_metadata();
// get_metadata function returns full path to the database file path, but
// ros2 bag seems to require relative file paths
diff --git a/main/src/vtr_storage/src/storage/sqlite/sqlite_storage.cpp b/main/src/vtr_storage/src/storage/sqlite/sqlite_storage.cpp
index 130fcecc3..756442bf6 100644
--- a/main/src/vtr_storage/src/storage/sqlite/sqlite_storage.cpp
+++ b/main/src/vtr_storage/src/storage/sqlite/sqlite_storage.cpp
@@ -179,7 +179,8 @@ void SqliteStorage::activate_transaction()
#if false
ROSBAG2_STORAGE_DEFAULT_PLUGINS_LOG_DEBUG_STREAM("begin transaction");
#endif
- database_->prepare_statement("BEGIN TRANSACTION;")->execute_and_reset();
+ database_->prepare_statement("BEGIN IMMEDIATE;")->execute_and_reset();
+ // database_->prepare_statement("BEGIN TRANSACTION;")->execute_and_reset(); ANTHONY
active_transaction_ = true;
}
@@ -201,7 +202,15 @@ void SqliteStorage::commit_transaction()
void SqliteStorage::write(const std::shared_ptr & message)
{
std::lock_guard db_lock(database_write_mutex_);
+
+ // Clear any active read snapshots on this connection
+ current_message_row_ = ReadQueryResult::Iterator(nullptr, ReadQueryResult::Iterator::POSITION_END);
+ message_result_ = ReadQueryResult(nullptr);
+ read_statement_ = nullptr;
+
+ activate_transaction();
write_locked(message);
+ commit_transaction();
}
void SqliteStorage::write(const std::vector> & messages)
@@ -212,15 +221,14 @@ void SqliteStorage::write(const std::vector & message)
@@ -576,6 +584,7 @@ void SqliteStorage::prepare_for_reading()
message_result_ = read_statement_->execute_query<
std::shared_ptr, rcutils_time_point_value_t, std::string, int>();
current_message_row_ = message_result_.begin();
+ read_statement_ = nullptr; // close the read transaction
}
void SqliteStorage::fill_topics_and_types()
diff --git a/main/src/vtr_storage/src/storage/sqlite/sqlite_wrapper.cpp b/main/src/vtr_storage/src/storage/sqlite/sqlite_wrapper.cpp
index 5d5a762af..3f9e5028a 100644
--- a/main/src/vtr_storage/src/storage/sqlite/sqlite_wrapper.cpp
+++ b/main/src/vtr_storage/src/storage/sqlite/sqlite_wrapper.cpp
@@ -60,6 +60,8 @@ SqliteWrapper::SqliteWrapper(
int rc = sqlite3_open_v2(
uri.c_str(), &db_ptr,
SQLITE_OPEN_READONLY | SQLITE_OPEN_NOMUTEX, nullptr);
+ //prepare_statement("PRAGMA schema_version;")->execute_and_reset();
+ sqlite3_busy_timeout(db_ptr, 5000);
if (rc != SQLITE_OK) {
std::stringstream errmsg;
errmsg << "Could not read-only open database. SQLite error (" <<
@@ -67,7 +69,6 @@ SqliteWrapper::SqliteWrapper(
throw SqliteException{errmsg.str()};
}
// throws an exception if the database is not valid.
- prepare_statement("PRAGMA schema_version;")->execute_and_reset();
} else {
int rc = sqlite3_open_v2(
uri.c_str(), &db_ptr,
@@ -80,6 +81,7 @@ SqliteWrapper::SqliteWrapper(
}
/// \note in rosbag2 the following "default" pragma has been replaced by
/// user customizable pragma
+ prepare_statement("PRAGMA busy_timeout = 5000;")->execute_and_reset();
prepare_statement("PRAGMA journal_mode = WAL;")->execute_and_reset();
prepare_statement("PRAGMA synchronous = NORMAL;")->execute_and_reset();
}
diff --git a/main/src/vtr_tactic/include/vtr_tactic/modules/memory/live_mem_manager_module.hpp b/main/src/vtr_tactic/include/vtr_tactic/modules/memory/live_mem_manager_module.hpp
index d78c92b2e..331730d07 100644
--- a/main/src/vtr_tactic/include/vtr_tactic/modules/memory/live_mem_manager_module.hpp
+++ b/main/src/vtr_tactic/include/vtr_tactic/modules/memory/live_mem_manager_module.hpp
@@ -18,6 +18,8 @@
*/
#pragma once
+#include
+
#include "vtr_tactic/modules/base_module.hpp"
#include "vtr_tactic/task_queue.hpp" /// include this header if using the task queue
@@ -54,8 +56,8 @@ class LiveMemManagerModule : public BaseModule {
const TaskExecutor::Ptr &, const Task::Priority &,
const Task::DepId &) override;
- /** \brief Module configuration. */
Config::ConstPtr config_;
+ std::deque vertex_window_;
VTR_REGISTER_MODULE_DEC_TYPE(LiveMemManagerModule);
};
diff --git a/main/src/vtr_tactic/include/vtr_tactic/rviz_tactic_callback.hpp b/main/src/vtr_tactic/include/vtr_tactic/rviz_tactic_callback.hpp
index 78bb4cc56..c75126fc7 100644
--- a/main/src/vtr_tactic/include/vtr_tactic/rviz_tactic_callback.hpp
+++ b/main/src/vtr_tactic/include/vtr_tactic/rviz_tactic_callback.hpp
@@ -22,6 +22,8 @@
#include
#include
#include
+#include
+#include
#include
#include
@@ -50,6 +52,8 @@ class RvizTacticCallback : virtual public TacticCallbackInterface {
private:
std::shared_ptr tf_bc_;
std::shared_ptr tf_static_bc_;
+ std::shared_ptr tf_buffer_;
+ std::shared_ptr tf_listener_;
rclcpp::Publisher::SharedPtr odometry_pub_;
rclcpp::Publisher::SharedPtr loc_path_pub_;
};
diff --git a/main/src/vtr_tactic/include/vtr_tactic/tactic.hpp b/main/src/vtr_tactic/include/vtr_tactic/tactic.hpp
index 9b4ab143f..0ac6ea8fe 100644
--- a/main/src/vtr_tactic/include/vtr_tactic/tactic.hpp
+++ b/main/src/vtr_tactic/include/vtr_tactic/tactic.hpp
@@ -21,6 +21,7 @@
#include "rclcpp/rclcpp.hpp"
#include "vtr_pose_graph/tools/pose_graph_smoother.hpp"
+#include "vtr_pose_graph/index/edge_base.hpp"
#include "vtr_tactic/cache.hpp"
#include "vtr_tactic/pipeline_interface.hpp"
#include "vtr_tactic/pipelines/base_pipeline.hpp"
@@ -29,7 +30,7 @@
#include "vtr_tactic/task_queue.hpp"
#include "vtr_tactic/types.hpp"
#include "geometry_msgs/msg/twist.hpp"
-
+#include "vtr_pose_graph_msgs/msg/edge.hpp"
namespace vtr {
namespace tactic {
@@ -42,7 +43,7 @@ class Tactic : public PipelineInterface, public TacticInterface {
using RobotStateMutex = std::mutex;
using RobotStateLock = std::unique_lock;
using RobotStateGuard = std::lock_guard;
-
+
struct Config {
PTR_TYPEDEFS(Config);
@@ -93,7 +94,7 @@ class Tactic : public PipelineInterface, public TacticInterface {
const EdgeTransform& T_twig_branch = EdgeTransform(true),
const bool publish = false) override;
void setTrunk(const VertexId& v = VertexId::Invalid()) override;
- void connectToTrunk(const bool privileged = false) override;
+ void connectToTrunk(const EdgeMode& privileged = EdgeMode::Autonomous) override;
/// \note following queries can be called without pipeline locked
Localization getPersistentLoc() const override;
bool isLocalized() const override;
@@ -128,7 +129,7 @@ class Tactic : public PipelineInterface, public TacticInterface {
private:
/// pipeline helper functions and states
void addVertexEdge(const Timestamp& stamp, const EdgeTransform& T_r_v,
- const bool manual, const EnvInfo& env_info);
+ const EdgeMode& mode, const EnvInfo& env_info);
/**
* \brief Whether this is the first frame of this run, only used by
@@ -156,6 +157,8 @@ class Tactic : public PipelineInterface, public TacticInterface {
*/
PipelineMode pipeline_mode_ = PipelineMode::Idle;
+ std::chrono::steady_clock::time_point last_load_live_{};
+
private:
Config::UniquePtr config_;
const BasePipeline::Ptr pipeline_;
diff --git a/main/src/vtr_tactic/include/vtr_tactic/tactic_interface.hpp b/main/src/vtr_tactic/include/vtr_tactic/tactic_interface.hpp
index 734b08af6..c222f2bae 100644
--- a/main/src/vtr_tactic/include/vtr_tactic/tactic_interface.hpp
+++ b/main/src/vtr_tactic/include/vtr_tactic/tactic_interface.hpp
@@ -53,7 +53,7 @@ class TacticInterface {
/** \brief Set the current privileged vertex (topological localization) */
virtual void setTrunk(const VertexId& v = VertexId::Invalid()) = 0;
/** \brief Add a new vertex, link it to the current trunk and branch */
- virtual void connectToTrunk(const bool privileged = false) = 0;
+ virtual void connectToTrunk(const EdgeMode& privileged = EdgeMode::Autonomous) = 0;
/** \brief Get the current persistent localization (i.e. curr robot loc) */
virtual Localization getPersistentLoc() const = 0;
/** \brief Whether robot has been localized successfully */
diff --git a/main/src/vtr_tactic/include/vtr_tactic/types.hpp b/main/src/vtr_tactic/include/vtr_tactic/types.hpp
index 0b5034858..13c44fc55 100644
--- a/main/src/vtr_tactic/include/vtr_tactic/types.hpp
+++ b/main/src/vtr_tactic/include/vtr_tactic/types.hpp
@@ -42,11 +42,14 @@ using Vertex = pose_graph::RCVertex;
using EdgeId = pose_graph::EdgeId;
using Edge = pose_graph::RCEdge;
using EdgeType = pose_graph::EdgeType;
+using EdgeMode = pose_graph::EdgeMode;
using EdgeTransform = pose_graph::EdgeTransform;
using LocalizationChain = pose_graph::LocalizationChain;
template
using PrivilegedEvaluator = pose_graph::eval::mask::privileged::Eval;
template
+using TopologyEvaluator = pose_graph::eval::mask::topology::Eval;
+template
using TemporalEvaluator = pose_graph::eval::mask::temporal::Eval;
template
using DistanceEvaluator = pose_graph::eval::weight::distance::Eval;
diff --git a/main/src/vtr_tactic/src/modules/memory/live_mem_manager_module.cpp b/main/src/vtr_tactic/src/modules/memory/live_mem_manager_module.cpp
index 16c7f039a..28590a7f7 100644
--- a/main/src/vtr_tactic/src/modules/memory/live_mem_manager_module.cpp
+++ b/main/src/vtr_tactic/src/modules/memory/live_mem_manager_module.cpp
@@ -35,16 +35,16 @@ void LiveMemManagerModule::run_(QueryCache &qdata, OutputCache &,
const Graph::Ptr &,
const TaskExecutor::Ptr &executor) {
if (qdata.vid_odo->isValid() &&
- qdata.vid_odo->minorId() >= (unsigned)config_->window_size &&
*qdata.vertex_test_result == VertexTestResult::CREATE_VERTEX) {
- const auto vid_to_unload =
- VertexId(qdata.vid_odo->majorId(),
- qdata.vid_odo->minorId() - (unsigned)config_->window_size);
- qdata.live_mem_async.emplace(vid_to_unload);
-
- executor->dispatch(std::make_shared(
- shared_from_this(), qdata.shared_from_this(), 0, Task::DepIdSet{},
- Task::DepId{}, "Live Mem Manager", vid_to_unload));
+ vertex_window_.push_back(*qdata.vid_odo);
+ if ((int)vertex_window_.size() > config_->window_size) {
+ const auto vid_to_unload = vertex_window_.front();
+ vertex_window_.pop_front();
+ qdata.live_mem_async.emplace(vid_to_unload);
+ executor->dispatch(std::make_shared(
+ shared_from_this(), qdata.shared_from_this(), 0, Task::DepIdSet{},
+ Task::DepId{}, "Live Mem Manager", vid_to_unload));
+ }
}
}
diff --git a/main/src/vtr_tactic/src/rviz_tactic_callback.cpp b/main/src/vtr_tactic/src/rviz_tactic_callback.cpp
index 0cd6788a4..a93193dfc 100644
--- a/main/src/vtr_tactic/src/rviz_tactic_callback.cpp
+++ b/main/src/vtr_tactic/src/rviz_tactic_callback.cpp
@@ -37,6 +37,8 @@ RvizTacticCallback::RvizTacticCallback(const rclcpp::Node::SharedPtr& node,
tf_static_bc_ = std::make_shared(node);
tf_bc_ = std::make_shared(node);
+ tf_buffer_ = std::make_shared(node->get_clock());
+ tf_listener_ = std::make_shared(*tf_buffer_);
odometry_pub_ = node->create_publisher("odometry", 10);
loc_path_pub_ = node->create_publisher("loc_path", 10);
diff --git a/main/src/vtr_tactic/src/storables.cpp b/main/src/vtr_tactic/src/storables.cpp
index fb8606d2d..cff475838 100644
--- a/main/src/vtr_tactic/src/storables.cpp
+++ b/main/src/vtr_tactic/src/storables.cpp
@@ -19,6 +19,9 @@
#include "vtr_tactic/storables.hpp"
#include "vtr_common/conversions/ros_lgmath.hpp"
+#include "vtr_pose_graph_msgs/msg/edge.hpp"
+#include "vtr_pose_graph_msgs/msg/edge_type.hpp"
+
namespace vtr {
namespace tactic {
diff --git a/main/src/vtr_tactic/src/tactic.cpp b/main/src/vtr_tactic/src/tactic.cpp
index b4472dc4a..ea786fd4e 100644
--- a/main/src/vtr_tactic/src/tactic.cpp
+++ b/main/src/vtr_tactic/src/tactic.cpp
@@ -52,6 +52,7 @@ auto Tactic::Config::fromROS(const rclcpp::Node::SharedPtr& node,
config->save_localization_result = node->declare_parameter(prefix+".save_localization_result", false);
config->visualize = node->declare_parameter(prefix+".visualize", false);
// clang-format on
+
return config;
}
@@ -73,7 +74,7 @@ Tactic::Tactic(Config::UniquePtr config, const BasePipeline::Ptr& pipeline,
output_->chain = chain_; // shared pointing to the same chain, no copy
output_->odometry_success.emplace(false);
//
- pipeline_->initialize(output_, graph_);
+ pipeline_->initialize(output_, graph_);
}
auto Tactic::lockPipeline() -> TacticInterface::PipelineLock {
@@ -85,7 +86,7 @@ void Tactic::setPipeline(const PipelineMode& pipeline_mode) {
pipeline_mode_ = pipeline_mode;
}
-void Tactic::addRun(const bool) {
+void Tactic::addRun(const bool ephemeral) {
graph_->addRun();
// re-initialize the run
first_frame_ = true;
@@ -100,16 +101,11 @@ void Tactic::addRun(const bool) {
T_m_w_ = EdgeTransform(true);
// re-initialize the pipeline
pipeline_->reset();
- //
callback_->startRun();
}
void Tactic::finishRun() {
- // saving graph here is optional as we save at destruction, just to avoid
- // unexpected data loss
smoother_.runBranchSmoothing();
- graph_->save();
- //
callback_->endRun();
}
@@ -122,16 +118,19 @@ void Tactic::setPath(const VertexId::Vector& path, const unsigned& trunk_sid,
const EdgeTransform& T_twig_branch, const bool publish) {
/// Set path and target localization
CLOG(INFO, "tactic") << "Set path of size " << path.size();
- ///
+ //
+ graph_->loadLive();
+
auto lock = chain_->guard();
//
chain_->setSequence(path);
if (path.size() > 0) {
chain_->expand();
auto eval =
- std::make_shared>(*graph_);
- auto connected = graph_->dijkstraSearch(VertexId(0, 0), path.front(), std::make_shared(1, 1), eval);
- T_m_w_ = pose_graph::eval::ComposeTfAccumulator(connected->beginDfs(VertexId(0, 0)), connected->end(), EdgeTransform(true));
+ std::make_shared>(*graph_);
+ const auto graph_root = graph_->root();
+ auto connected = graph_->dijkstraSearch(graph_root, path.front(), std::make_shared(1, 1), eval);
+ T_m_w_ = pose_graph::eval::ComposeTfAccumulator(connected->beginDfs(graph_root), connected->end(), EdgeTransform(true));
CLOG(INFO, "tactic") << "Setting tf from root to " << T_m_w_;
}
// used as initial guess for trunk
@@ -157,7 +156,7 @@ void Tactic::setTrunk(const VertexId& v) {
callback_->robotStateUpdated(persistent_loc_, target_loc_);
}
-void Tactic::connectToTrunk(const bool privileged) {
+void Tactic::connectToTrunk(const EdgeMode& privileged) {
const auto [twig_vid, branch_vid, T_twig_branch] = [&]() {
auto lock = chain_->guard();
return std::make_tuple(chain_->twigVertexId(), chain_->branchVertexId(),
@@ -165,7 +164,7 @@ void Tactic::connectToTrunk(const bool privileged) {
}();
CLOG(INFO, "tactic") << "Adding connection " << twig_vid << " --> "
<< branch_vid << ", privileged: " << std::boolalpha
- << privileged << ", with T_to_from: "
+ << static_cast(privileged) << ", with T_to_from: "
<< T_twig_branch.inverse().vec().transpose();
graph_->addEdge(twig_vid, branch_vid, EdgeType::Spatial, privileged,
T_twig_branch.inverse());
@@ -208,6 +207,11 @@ bool Tactic::routeCompleted() const {
}
bool Tactic::input_(const QueryCache::Ptr&) {
+ auto now = std::chrono::steady_clock::now();
+ if (now - last_load_live_ >= std::chrono::milliseconds(2000)) {
+ last_load_live_ = now;
+ graph_->loadLive();
+ }
return config_->preprocessing_skippable;
}
@@ -277,6 +281,13 @@ bool Tactic::teachMetricLocOdometryMapping(const QueryCache::Ptr& qdata) {
auto msg = std::make_shared(odo_result, *qdata->stamp);
graph_->write("odometry_result",
"vtr_tactic_msgs/msg/OdometryResult", msg);
+
+ // using edgeLM = storage::LockableMessage;
+ // auto edg_result = std::make_shared(
+ // *qdata->stamp, T_w_v_odo_ * (*qdata->T_r_v_odo).inverse());
+ // auto edge_msg = std::make_shared(edg_result, *qdata->stamp);
+ // graph_->write("edge",
+ // "vtr_posegraph_msgs/msg/Edge", edge_msg);
}
// save odometry velocity result
@@ -323,7 +334,7 @@ bool Tactic::teachMetricLocOdometryMapping(const QueryCache::Ptr& qdata) {
const auto& vertex_test_result = *qdata->vertex_test_result;
if (vertex_test_result == VertexTestResult::CREATE_VERTEX || force_add_vertex_) {
// Add new vertex to the posegraph
- addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), true,
+ addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), EdgeMode::Manual,
*(qdata->env_info));
CLOG(INFO, "tactic") << "Creating a new vertex with id "
<< current_vertex_id_;
@@ -352,6 +363,9 @@ bool Tactic::teachMetricLocOdometryMapping(const QueryCache::Ptr& qdata) {
qdata->sid_loc.emplace(chain_->trunkSequenceId());
qdata->T_r_v_loc.emplace(chain_->T_leaf_trunk());
+
+ // graph_->save(); // saveLive handles
+
return config_->localization_skippable;
}
@@ -425,7 +439,7 @@ bool Tactic::teachBranchOdometryMapping(const QueryCache::Ptr& qdata) {
const auto& vertex_test_result = *qdata->vertex_test_result;
if (vertex_test_result == VertexTestResult::CREATE_VERTEX || force_add_vertex_) {
// Add new vertex to the posegraph
- addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), true,
+ addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), EdgeMode::Manual,
*(qdata->env_info));
CLOG(INFO, "tactic") << "Creating a new vertex with id "
<< current_vertex_id_;
@@ -521,7 +535,7 @@ bool Tactic::teachMergeOdometryMapping(const QueryCache::Ptr& qdata) {
const auto& vertex_test_result = *qdata->vertex_test_result;
if (vertex_test_result == VertexTestResult::CREATE_VERTEX) {
// Add new vertex to the posegraph
- addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), true,
+ addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), EdgeMode::Manual,
*(qdata->env_info));
CLOG(INFO, "tactic") << "Creating a new vertex with id "
<< current_vertex_id_;
@@ -593,7 +607,7 @@ bool Tactic::repeatMetricLocOdometryMapping(const QueryCache::Ptr& qdata) {
const auto& vertex_test_result = *qdata->vertex_test_result;
if (vertex_test_result == VertexTestResult::CREATE_VERTEX) {
// Add new vertex to the posegraph
- addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), false,
+ addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), EdgeMode::Autonomous,
*(qdata->env_info));
CLOG(INFO, "tactic") << "Creating a new vertex with id "
<< current_vertex_id_;
@@ -663,7 +677,7 @@ bool Tactic::repeatFollowOdometryMapping(const QueryCache::Ptr& qdata) {
const auto& vertex_test_result = *qdata->vertex_test_result;
if (vertex_test_result == VertexTestResult::CREATE_VERTEX) {
// Add new vertex to the posegraph
- addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), false,
+ addVertexEdge(*(qdata->stamp), *(qdata->T_r_v_odo), EdgeMode::Autonomous,
*(qdata->env_info));
CLOG(INFO, "tactic") << "Creating a new vertex with id "
<< current_vertex_id_;
@@ -901,7 +915,8 @@ bool Tactic::repeatFollowLocalization(const QueryCache::Ptr& qdata) {
auto msg = std::make_shared(loc_result, *qdata->stamp);
graph_->write(
"localization_result", "vtr_tactic_msgs/msg/LocalizationResult", msg);
- }
+
+}
if (!(*qdata->loc_success)) {
CLOG(WARNING, "tactic") << "Localization failed, skip updating pose graph "
@@ -924,7 +939,7 @@ bool Tactic::repeatFollowLocalization(const QueryCache::Ptr& qdata) {
<< *(qdata->vid_odo) << " and " << *(qdata->vid_loc)
<< " to the graph.";
graph_->addEdge(*(qdata->vid_odo), *(qdata->vid_loc), EdgeType::Spatial,
- false, T_v_odo_loc.inverse());
+ EdgeMode::Autonomous, T_v_odo_loc.inverse());
CLOG(DEBUG, "tactic") << "Done adding the spatial edge between "
<< *(qdata->vid_odo) << " and " << *(qdata->vid_loc)
<< " to the graph.";
@@ -982,7 +997,7 @@ bool Tactic::localizeMetricLocLocalization(const QueryCache::Ptr& qdata) {
}
void Tactic::addVertexEdge(const Timestamp& stamp, const EdgeTransform& T_r_v,
- const bool manual, const EnvInfo& env_info) {
+ const EdgeMode& mode, const EnvInfo& env_info) {
//
const auto previous_vertex_id = current_vertex_id_;
@@ -1004,8 +1019,11 @@ void Tactic::addVertexEdge(const Timestamp& stamp, const EdgeTransform& T_r_v,
// Add the new edge
if (!previous_vertex_id.isValid()) return;
- (void)graph_->addEdge(previous_vertex_id, current_vertex_id_,
- EdgeType::Temporal, manual, T_r_v);
+ auto edge = graph_->addEdge(previous_vertex_id, current_vertex_id_,
+ EdgeType::Temporal, mode, T_r_v);
+
+ // Write new data to disk
+ graph_->saveLive();
}
void Tactic::updatePersistentLoc(const Timestamp& t, const VertexId& v,