WIP | Port package to ROS2

.dockerignore

@@ -0,0 +1,8 @@
+################################################################################
+# Repo
+
+.git/*
+.dockerignore
+.gitignore
+**Dockerfile
+**.Dockerfile

Dockerfile

@@ -0,0 +1,55 @@
+ARG FROM_IMAGE=ros:eloquent
+
+# multi-stage for caching
+FROM $FROM_IMAGE AS cache
+
+# copy overlay source
+ENV OVERLAY_WS /opt/overlay_ws
+RUN mkdir -p $OVERLAY_WS/src
+WORKDIR $OVERLAY_WS
+COPY ./ src/pose_graph_tools
+
+# copy manifests for caching
+WORKDIR /opt
+RUN find ./ -name "package.xml" | \
+      xargs cp --parents -t /tmp
+    # && find ./ -name "COLCON_IGNORE" | \
+    #   xargs cp --parents -t /tmp
+
+# multi-stage for building
+FROM $FROM_IMAGE AS build
+
+# install CI dependencies
+RUN apt-get update && apt-get install -q -y \
+      ccache \
+      lcov \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy overlay manifests
+ENV OVERLAY_WS /opt/overlay_ws
+COPY --from=cache /tmp/overlay_ws $OVERLAY_WS
+WORKDIR $OVERLAY_WS
+
+# install overlay dependencies
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+    apt-get update && rosdep install -q -y \
+      --from-paths \
+        src \
+      --ignore-src \
+    && rm -rf /var/lib/apt/lists/*
+
+# copy overlay source
+COPY --from=cache $OVERLAY_WS ./
+
+# build overlay source
+ARG OVERLAY_MIXINS="release ccache"
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+    colcon build \
+      --symlink-install \
+      --mixin $OVERLAY_MIXINS
+      # --event-handlers console_direct+
+
+# source overlay from entrypoint
+RUN sed --in-place \
+      's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \
+      /ros_entrypoint.sh

pose_graph_msgs/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(pose_graph_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+set(msg_files
+  "msg/PoseGraph.msg"
+  "msg/PoseGraphEdge.msg"
+  "msg/PoseGraphNode.msg")
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${msg_files}
+  DEPENDENCIES
+  builtin_interfaces
+  std_msgs
+  geometry_msgs)
+
+ament_package()

pose_graph_msgs/msg/PoseGraph.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+# Nodes and edges
+PoseGraphNode[] nodes
+PoseGraphEdge[] edges

msg/PoseGraphEdge.msg → pose_graph_msgs/msg/PoseGraphEdge.msg

@@ -1,4 +1,4 @@
-Header header
+std_msgs/Header header
 
 uint64 key_from
 uint64 key_to
@@ -13,4 +13,4 @@ int32 REJECTED_LOOPCLOSE = 3
 
 # Transforms in ede
 geometry_msgs/Pose pose
-float64[36] covariance
+float64[36] covariance

pose_graph_msgs/msg/PoseGraphNode.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+uint64 key
+
+geometry_msgs/Pose pose

pose_graph_msgs/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pose_graph_msgs</name>
+  <version>1.0.0</version>
+  <description>The pose_graph_msgs package</description>
+
+  <maintainer email="[email protected]">Yun Chang</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

pose_graph_visualizer/CMakeLists.txt

@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(pose_graph_visualizer)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(pose_graph_msgs REQUIRED)
+find_package(interactive_markers REQUIRED)
+
+include_directories(
+  include
+)
+
+set(executable_name visualizer_node)
+add_executable(${executable_name}
+  src/visualizer.cpp
+  src/visualizer_node.cpp
+)
+
+set(library_name ${PROJECT_NAME})
+add_library(${library_name} SHARED
+  src/visualizer.cpp
+)
+
+set(dependencies
+  geometry_msgs
+  rclcpp
+  visualization_msgs
+  pose_graph_msgs
+  interactive_markers
+)
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+target_link_libraries(${executable_name} ${library_name})
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+install(TARGETS ${executable_name} ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+
+ament_package()

pose_graph_visualizer/include/pose_graph_visualizer/visualizer.hpp

@@ -0,0 +1,52 @@
+#ifndef VISUALIZER_H
+#define VISUALIZER_H
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "geometry_msgs/msg/point.hpp"
+#include "visualization_msgs/msg/marker.hpp"
+
+#include "interactive_markers/interactive_marker_server.hpp"
+
+#include "pose_graph_msgs/msg/pose_graph.hpp"
+#include "pose_graph_msgs/msg/pose_graph_edge.hpp"
+#include "pose_graph_msgs/msg/pose_graph_node.hpp"
+
+#include <unordered_map>
+
+class Visualizer : public rclcpp::Node {
+public:
+	Visualizer();
+
+	void visualize();
+
+private:
+	void PoseGraphCallback(const pose_graph_msgs::msg::PoseGraph::SharedPtr msg);
+
+	geometry_msgs::msg::Point getPositionFromKey(long unsigned int key) const;
+
+	 void MakeMenuMarker(const geometry_msgs::msg::Pose &position, const std::string &id_number);
+
+private:
+	std::string frame_id_;
+
+	// subscribers
+	rclcpp::Subscription<pose_graph_msgs::msg::PoseGraph>::SharedPtr pose_graph_sub_;
+
+	// publishers
+	rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr odometry_edge_pub_;
+	rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr loop_edge_pub_;
+	rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr rejected_loop_edge_pub_;
+	rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr graph_node_pub_;
+	rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr graph_node_id_pub_;
+
+	typedef std::pair<long unsigned int, long unsigned int> Edge;
+	std::vector<Edge> odometry_edges_;
+	std::vector<Edge> loop_edges_;
+	std::vector<Edge> rejected_loop_edges_;
+	std::unordered_map<long unsigned int, geometry_msgs::msg::Pose> keyed_poses_;
+
+  	std::shared_ptr<interactive_markers::InteractiveMarkerServer> interactive_mrkr_srvr_;
+};
+
+#endif

launch/posegraph_view.launch → pose_graph_visualizer/launch/posegraph_view.launch.xml

@@ -4,7 +4,7 @@
   <arg name="ns"          default="/pose_graph"/>
 
   <!-- visualizer node -->
-  <node name="posegraph_viewer" pkg="pose_graph_tools" type="visualizer_node"
+  <node name="posegraph_viewer" pkg="pose_graph_visualizer" exec="visualizer_node"
     output="screen" ns="$(arg ns)">
     <remap from="graph"       to="$(arg graph_topic)" />