diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..dca6942 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,8 @@ +################################################################################ +# Repo + +.git/* +.dockerignore +.gitignore +**Dockerfile +**.Dockerfile diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..52a1971 --- /dev/null +++ b/.gitignore @@ -0,0 +1,52 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE + diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..a5c76a7 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,162 @@ +ARG FROM_IMAGE=ros:eloquent + +# multi-stage for caching +FROM $FROM_IMAGE AS cache + +# clone underlay source +ENV UNDERLAY_WS /opt/underlay_ws +RUN mkdir -p $UNDERLAY_WS/src +WORKDIR $UNDERLAY_WS +COPY ./install/underlay.repos ./ +RUN vcs import src < underlay.repos + +# copy overlay source +ENV OVERLAY_WS /opt/overlay_ws +RUN mkdir -p $OVERLAY_WS/src +WORKDIR $OVERLAY_WS +COPY ./install/overlay.repos ./ +RUN vcs import src < overlay.repos + +# copy wrapper source +ENV WRAPPER_WS /opt/wrapper_ws +RUN mkdir -p $WRAPPER_WS/src +WORKDIR $WRAPPER_WS +# COPY ./install/wrapper.repos ./ +# RUN vcs import src < wrapper.repos +COPY ./ src/MIT-SPARK/Kimera-VIO-ROS2 + +# 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/* + +# install opencv dependencies +RUN apt-get update && apt-get install -y \ + gfortran \ + libatlas-base-dev \ + libgtk-3-dev \ + libjpeg-dev \ + libpng-dev \ + libtiff-dev \ + libvtk6-dev \ + unzip \ + && rm -rf /var/lib/apt/lists/* + +# copy underlay manifests +ENV UNDERLAY_WS /opt/underlay_ws +COPY --from=cache /tmp/underlay_ws $UNDERLAY_WS +WORKDIR $UNDERLAY_WS + +# install underlay 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 underlay source +COPY --from=cache $UNDERLAY_WS ./ + +# build underlay source +ARG UNDERLAY_MIXINS="release" +RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ + colcon build \ + --symlink-install \ + --mixin $UNDERLAY_MIXINS \ + --cmake-args \ + --no-warn-unused-cli \ + -DGTSAM_BUILD_TESTS=OFF \ + -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ + -DGTSAM_BUILD_UNSTABLE=ON \ + -DGTSAM_POSE3_EXPMAP=ON \ + -DGTSAM_ROT3_EXPMAP=ON \ + -DOPENCV_EXTRA_MODULES_PATH=$UNDERLAY_WS/src/opencv/opencv/contrib/modules + # --event-handlers console_direct+ + +# copy overlay manifests +ENV OVERLAY_WS /opt/overlay_ws +COPY --from=cache /tmp/overlay_ws $OVERLAY_WS +WORKDIR $OVERLAY_WS + +# install overlay dependencies +RUN . $UNDERLAY_WS/install/setup.sh && \ + apt-get update && rosdep install -q -y \ + --from-paths \ + src \ + $UNDERLAY_WS/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 . $UNDERLAY_WS/install/setup.sh && \ + colcon build \ + --symlink-install \ + --mixin $OVERLAY_MIXINS \ + --cmake-args \ + --no-warn-unused-cli \ + -DCMAKE_CXX_FLAGS="\ + -Wno-parentheses \ + -Wno-reorder \ + -Wno-sign-compare \ + -Wno-unused-but-set-variable \ + -Wno-unused-function \ + -Wno-unused-parameter \ + -Wno-unused-value \ + -Wno-unused-variable" + # --event-handlers console_direct+ + +# copy wrapper manifests +ENV WRAPPER_WS /opt/wrapper_ws +COPY --from=cache /tmp/wrapper_ws $WRAPPER_WS +WORKDIR $WRAPPER_WS + +# install wrapper dependencies +RUN . $OVERLAY_WS/install/setup.sh && \ + apt-get update && rosdep install -q -y \ + --from-paths \ + src \ + $UNDERLAY_WS/src \ + $OVERLAY_WS/src \ + --ignore-src \ + && rm -rf /var/lib/apt/lists/* + +# copy wrapper source +COPY --from=cache $WRAPPER_WS ./ + +# build wrapper source +ARG WRAPPER_MIXINS="release ccache" +RUN . $OVERLAY_WS/install/setup.sh && \ + colcon build \ + --symlink-install \ + --mixin $WRAPPER_MIXINS \ + --cmake-args \ + --no-warn-unused-cli \ + -DCMAKE_CXX_FLAGS="\ + -Wno-sign-compare \ + -Wno-unused-value \ + -Wno-unused-variable \ + -Wno-unused-but-set-variable \ + -Wno-reorder \ + -Wno-parentheses \ + -Wno-unused-parameter" + # --event-handlers console_direct+ + +# source wrapper from entrypoint +RUN sed --in-place \ + 's|^source .*|source "$WRAPPER_WS/install/setup.bash"|' \ + /ros_entrypoint.sh diff --git a/install/overlay.repos b/install/overlay.repos new file mode 100644 index 0000000..5c11197 --- /dev/null +++ b/install/overlay.repos @@ -0,0 +1,13 @@ +repositories: + MIT-SPARK/Kimera-RPGO: + type: git + # url: https://github.com/MIT-SPARK/Kimera-RPGO.git + # version: master + url: https://github.com/ruffsl/Kimera-RPGO.git + version: patch-1 + MIT-SPARK/Kimera-VIO: + type: git + # url: https://github.com/MIT-SPARK/Kimera-VIO.git + # version: master + url: https://github.com/ruffsl/Kimera-VIO.git + version: patch-3 diff --git a/install/underlay.repos b/install/underlay.repos new file mode 100644 index 0000000..8f8bbc6 --- /dev/null +++ b/install/underlay.repos @@ -0,0 +1,27 @@ +repositories: + borglab/gtsam: + type: git + url: https://github.com/borglab/gtsam.git + version: develop + laurentkneip/opengv: + type: git + # url: https://github.com/laurentkneip/opengv.git + # version: master + url: https://github.com/ruffsl/opengv.git + version: patch-1 + dorian3d/DBoW2: + type: git + # url: https://github.com/dorian3d/DBoW2.git + # version: master + url: https://github.com/ruffsl/DBoW2.git + version: patch-1 + opencv/opencv: + type: git + # url: https://github.com/opencv/opencv.git + # version: master + url: https://github.com/ruffsl/opencv.git + version: patch-1 + opencv/opencv/contrib: + type: git + url: https://github.com/opencv/opencv_contrib.git + version: 3.3.1 diff --git a/install/wrapper.repos b/install/wrapper.repos new file mode 100644 index 0000000..3b27842 --- /dev/null +++ b/install/wrapper.repos @@ -0,0 +1,15 @@ +repositories: + MIT-SPARK/Kimera-VIO-ROS2: + type: git + url: https://github.com/MIT-SPARK/Kimera-VIO-ROS2.git + version: master + # ToniRV/mesh_rviz_plugins: + # type: git + # url: https://github.com/ToniRV/mesh_rviz_plugins.git + # version: master + # # url: https://github.com/ruffsl/mesh_rviz_plugins.git + # # version: patch-1 + # ToniRV/kimera_rviz_markers: + # type: git + # url: https://github.com/ToniRV/kimera_rviz_markers.git + # version: master diff --git a/kimera_common/CMakeLists.txt b/kimera_common/CMakeLists.txt new file mode 100644 index 0000000..220fcbc --- /dev/null +++ b/kimera_common/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.5) +project(kimera_common NONE) + +find_package(ament_cmake_core REQUIRED) + +ament_package( + CONFIG_EXTRAS "kimera_common-extras.cmake" +) + +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) diff --git a/kimera_common/cmake/kimera_package.cmake b/kimera_common/cmake/kimera_package.cmake new file mode 100644 index 0000000..a836427 --- /dev/null +++ b/kimera_common/cmake/kimera_package.cmake @@ -0,0 +1,22 @@ +# +# Standard Kimera project setup +# +# @public +# +macro(kimera_package) + + # 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 + -Werror + -Wdeprecated + ) + endif() +endmacro() diff --git a/kimera_common/kimera_common-extras.cmake b/kimera_common/kimera_common-extras.cmake new file mode 100644 index 0000000..092af8f --- /dev/null +++ b/kimera_common/kimera_common-extras.cmake @@ -0,0 +1,3 @@ +set(AMENT_BUILD_CONFIGURATION_KEYWORD_SEPARATOR ":") + +include("${kimera_common_DIR}/kimera_package.cmake") diff --git a/kimera_common/package.xml b/kimera_common/package.xml new file mode 100644 index 0000000..ebd87fb --- /dev/null +++ b/kimera_common/package.xml @@ -0,0 +1,16 @@ + + + kimera_common + 0.0.1 + Common support functionality used throughout the Kimera stack + Ruffin White + BSD + + ament_cmake_core + + ament_cmake_core + + + ament_cmake + + diff --git a/kimera_graph_msgs/CMakeLists.txt b/kimera_graph_msgs/CMakeLists.txt new file mode 100644 index 0000000..890c965 --- /dev/null +++ b/kimera_graph_msgs/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.5) +project(kimera_graph_msgs) + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(kimera_common REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +kimera_package() + +set(msg_files + "msg/PoseGraph.msg" + "msg/PoseGraphEdge.msg" + "msg/PoseGraphNode.msg" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES + builtin_interfaces + geometry_msgs + std_msgs +) + +ament_package() diff --git a/kimera_graph_msgs/msg/PoseGraph.msg b/kimera_graph_msgs/msg/PoseGraph.msg new file mode 100644 index 0000000..bea8cfc --- /dev/null +++ b/kimera_graph_msgs/msg/PoseGraph.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +# Nodes and edges +PoseGraphNode[] nodes +PoseGraphEdge[] edges diff --git a/kimera_graph_msgs/msg/PoseGraphEdge.msg b/kimera_graph_msgs/msg/PoseGraphEdge.msg new file mode 100644 index 0000000..5b275ff --- /dev/null +++ b/kimera_graph_msgs/msg/PoseGraphEdge.msg @@ -0,0 +1,16 @@ +std_msgs/Header header + +uint64 key_from +uint64 key_to + +int32 type + +# Type enums +int32 ODOM = 0 +int32 LOOPCLOSE = 1 +int32 LANDMARK = 2 +int32 REJECTED_LOOPCLOSE = 3 + +# Transforms in ede +geometry_msgs/Pose pose +float64[36] covariance diff --git a/kimera_graph_msgs/msg/PoseGraphNode.msg b/kimera_graph_msgs/msg/PoseGraphNode.msg new file mode 100644 index 0000000..1d55100 --- /dev/null +++ b/kimera_graph_msgs/msg/PoseGraphNode.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +uint64 key + +geometry_msgs/Pose pose diff --git a/kimera_graph_msgs/package.xml b/kimera_graph_msgs/package.xml new file mode 100644 index 0000000..3ae788c --- /dev/null +++ b/kimera_graph_msgs/package.xml @@ -0,0 +1,28 @@ + + + kimera_graph_msgs + 1.0.0 + The kimera_graph_msgs package + + Yun Chang + + BSD + + ament_cmake + + geometry_msgs + kimera_common + std_msgs + + builtin_interfaces + rosidl_default_generators + + builtin_interfaces + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/kimera_graph_visualizer/CMakeLists.txt b/kimera_graph_visualizer/CMakeLists.txt new file mode 100644 index 0000000..dd83121 --- /dev/null +++ b/kimera_graph_visualizer/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.5) +project(kimera_graph_visualizer) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(kimera_common REQUIRED) +find_package(kimera_graph_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(visualization_msgs REQUIRED) + +kimera_package() + +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 + interactive_markers + kimera_graph_msgs + rclcpp + visualization_msgs +) + +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) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) + +ament_package() diff --git a/kimera_graph_visualizer/include/kimera_graph_visualizer/visualizer.hpp b/kimera_graph_visualizer/include/kimera_graph_visualizer/visualizer.hpp new file mode 100644 index 0000000..4fd4683 --- /dev/null +++ b/kimera_graph_visualizer/include/kimera_graph_visualizer/visualizer.hpp @@ -0,0 +1,53 @@ +#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 "kimera_graph_msgs/msg/pose_graph.hpp" +#include "kimera_graph_msgs/msg/pose_graph_edge.hpp" +#include "kimera_graph_msgs/msg/pose_graph_node.hpp" + +#include + +class Visualizer : public rclcpp::Node +{ +public: + Visualizer(); + + void visualize(); + +private: + void PoseGraphCallback(const kimera_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::SharedPtr pose_graph_sub_; + + // publishers + rclcpp::Publisher::SharedPtr odometry_edge_pub_; + rclcpp::Publisher::SharedPtr loop_edge_pub_; + rclcpp::Publisher::SharedPtr rejected_loop_edge_pub_; + rclcpp::Publisher::SharedPtr graph_node_pub_; + rclcpp::Publisher::SharedPtr graph_node_id_pub_; + + typedef std::pair Edge; + std::vector odometry_edges_; + std::vector loop_edges_; + std::vector rejected_loop_edges_; + std::unordered_map keyed_poses_; + + std::shared_ptr interactive_mrkr_srvr_; +}; + +#endif diff --git a/kimera_graph_visualizer/launch/posegraph_view.launch.xml b/kimera_graph_visualizer/launch/posegraph_view.launch.xml new file mode 100644 index 0000000..48bf963 --- /dev/null +++ b/kimera_graph_visualizer/launch/posegraph_view.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + diff --git a/kimera_graph_visualizer/package.xml b/kimera_graph_visualizer/package.xml new file mode 100644 index 0000000..c36cc34 --- /dev/null +++ b/kimera_graph_visualizer/package.xml @@ -0,0 +1,26 @@ + + + kimera_graph_visualizer + 1.0.0 + The kimera_graph_visualizer package + + Yun Chang + + BSD + + ament_cmake + + geometry_msgs + interactive_markers + kimera_common + kimera_graph_msgs + rclcpp + visualization_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/kimera_graph_visualizer/src/visualizer.cpp b/kimera_graph_visualizer/src/visualizer.cpp new file mode 100644 index 0000000..d613ce2 --- /dev/null +++ b/kimera_graph_visualizer/src/visualizer.cpp @@ -0,0 +1,243 @@ +#include "kimera_graph_visualizer/visualizer.hpp" +#include "interactive_markers/menu_handler.hpp" + +using std::placeholders::_1; + +Visualizer::Visualizer() +: Node("visualizer") +{ + RCLCPP_INFO(this->get_logger(), "Initializing pose graph visualizer"); + + // get parameters + if (!this->get_parameter("frame_id", frame_id_)) { + RCLCPP_ERROR(this->get_logger(), "Did not load frame id"); + } + + // start subscribers + pose_graph_sub_ = this->create_subscription( + "graph", 10, std::bind(&Visualizer::PoseGraphCallback, this, _1)); + + // start publishers + odometry_edge_pub_ = this->create_publisher( + "odometry_edges", 10); + loop_edge_pub_ = this->create_publisher( + "loop_edges", 10); + rejected_loop_edge_pub_ = this->create_publisher( + "rejected_loop_edges", 10); + + graph_node_pub_ = this->create_publisher( + "graph_nodes", 10); + graph_node_id_pub_ = this->create_publisher( + "graph_nodes_ids", 10); + + interactive_mrkr_srvr_ = std::make_shared( + "interactive_node", this); +} + +void Visualizer::PoseGraphCallback(const kimera_graph_msgs::msg::PoseGraph::SharedPtr msg) +{ + // iterate through nodes in pose graph + for (const kimera_graph_msgs::msg::PoseGraphNode & msg_node : msg->nodes) { + // Fill pose nodes (representing the robot position) + keyed_poses_[msg_node.key] = msg_node.pose; + } + + // iterate through edges in pose graph + for (const kimera_graph_msgs::msg::PoseGraphEdge & msg_edge : msg->edges) { + if (msg_edge.type == kimera_graph_msgs::msg::PoseGraphEdge::ODOM) { + odometry_edges_.emplace_back( + std::make_pair(msg_edge.key_from, msg_edge.key_to)); + } else if (msg_edge.type == kimera_graph_msgs::msg::PoseGraphEdge::LOOPCLOSE) { + loop_edges_.emplace_back( + std::make_pair(msg_edge.key_from, msg_edge.key_to)); + } else if (msg_edge.type == kimera_graph_msgs::msg::PoseGraphEdge::REJECTED_LOOPCLOSE) { + rejected_loop_edges_.emplace_back( + std::make_pair(msg_edge.key_from, msg_edge.key_to)); + } + } + + visualize(); +} + +geometry_msgs::msg::Point Visualizer::getPositionFromKey( + long unsigned int key) const +{ + auto v = keyed_poses_.at(key); + geometry_msgs::msg::Point p; + p.x = v.position.x; + p.y = v.position.y; + p.z = v.position.z; + return p; +} + +// Interactive Marker Menu to click and see key of node +void Visualizer::MakeMenuMarker( + const geometry_msgs::msg::Pose & position, + const std::string & id_number) +{ + interactive_markers::MenuHandler menu_handler; + + visualization_msgs::msg::InteractiveMarker int_marker; + int_marker.header.frame_id = frame_id_; + int_marker.scale = 1.0; + int_marker.pose = position; + int_marker.name = id_number; + + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.scale.x = 0.3; + marker.scale.y = 0.3; + marker.scale.z = 0.3; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 0.5; + marker.pose.orientation.w = 1.0; + + visualization_msgs::msg::InteractiveMarkerControl control; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MENU; + control.name = id_number; + control.markers.push_back(marker); + control.always_visible = true; + int_marker.controls.push_back(control); + + menu_handler.insert(id_number); + interactive_mrkr_srvr_->insert(int_marker); + menu_handler.apply(*interactive_mrkr_srvr_, int_marker.name); +} + + +void Visualizer::visualize() +{ + // Publish odometry edges. + if (odometry_edge_pub_->get_subscription_count() > 0) { + visualization_msgs::msg::Marker m; + m.header.frame_id = frame_id_; + m.ns = frame_id_; + m.id = 0; + m.action = visualization_msgs::msg::Marker::ADD; + m.type = visualization_msgs::msg::Marker::LINE_LIST; + m.color.r = 1.0; + m.color.g = 0.0; + m.color.b = 0.0; + m.color.a = 0.8; + m.scale.x = 0.02; + m.pose.orientation.w = 1.0; + + for (size_t ii = 0; ii < odometry_edges_.size(); ++ii) { + const auto key1 = odometry_edges_[ii].first; + const auto key2 = odometry_edges_[ii].second; + + m.points.push_back(getPositionFromKey(key1)); + m.points.push_back(getPositionFromKey(key2)); + } + odometry_edge_pub_->publish(m); + } + + // Publish loop closure edges. + if (loop_edge_pub_->get_subscription_count() > 0) { + visualization_msgs::msg::Marker m; + m.header.frame_id = frame_id_; + m.ns = frame_id_; + m.id = 1; + m.action = visualization_msgs::msg::Marker::ADD; + m.type = visualization_msgs::msg::Marker::LINE_LIST; + m.color.r = 0.0; + m.color.g = 0.2; + m.color.b = 1.0; + m.color.a = 0.8; + m.scale.x = 0.02; + m.pose.orientation.w = 1.0; + + for (size_t ii = 0; ii < loop_edges_.size(); ++ii) { + const auto key1 = loop_edges_[ii].first; + const auto key2 = loop_edges_[ii].second; + + m.points.push_back(getPositionFromKey(key1)); + m.points.push_back(getPositionFromKey(key2)); + } + loop_edge_pub_->publish(m); + } + + // Publish the rejected loop closure edges + if (rejected_loop_edge_pub_->get_subscription_count() > 0) { + visualization_msgs::msg::Marker m; + m.header.frame_id = frame_id_; + m.ns = frame_id_; + m.id = 1; + m.action = visualization_msgs::msg::Marker::ADD; + m.type = visualization_msgs::msg::Marker::LINE_LIST; + m.color.r = 0.5; + m.color.g = 0.5; + m.color.b = 0.5; + m.color.a = 0.7; + m.scale.x = 0.02; + m.pose.orientation.w = 1.0; + + for (size_t ii = 0; ii < rejected_loop_edges_.size(); ++ii) { + const auto key1 = rejected_loop_edges_[ii].first; + const auto key2 = rejected_loop_edges_[ii].second; + + m.points.push_back(getPositionFromKey(key1)); + m.points.push_back(getPositionFromKey(key2)); + } + rejected_loop_edge_pub_->publish(m); + } + + // Publish node IDs in the pose graph. + if (graph_node_id_pub_->get_subscription_count() > 0) { + visualization_msgs::msg::Marker m; + m.header.frame_id = frame_id_; + m.ns = frame_id_; + + m.action = visualization_msgs::msg::Marker::ADD; + m.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + m.color.r = 1.0; + m.color.g = 1.0; + m.color.b = 0.2; + m.color.a = 0.8; + m.scale.z = 0.01; // Only Scale z is used - height of capital A in the text + m.pose.orientation.w = 1.0; + + int id_base = 100; + for (const auto & keyedPose : keyed_poses_) { + m.pose = keyedPose.second; + // Display text for the node + m.text = std::to_string(keyedPose.first); + m.id = id_base + keyedPose.first; + graph_node_id_pub_->publish(m); + } + + // publish the interactive click-and-see key markers + for (const auto & keyedPose : keyed_poses_) { + std::string robot_id = std::to_string(keyedPose.first); + MakeMenuMarker(keyedPose.second, robot_id); + } + if (interactive_mrkr_srvr_ != nullptr) { + interactive_mrkr_srvr_->applyChanges(); + } + } + + // Publish keyframe nodes in the pose graph. + if (graph_node_pub_->get_subscription_count() > 0) { + visualization_msgs::msg::Marker m; + m.header.frame_id = frame_id_; + m.ns = frame_id_; + m.id = 4; + m.action = visualization_msgs::msg::Marker::ADD; + m.type = visualization_msgs::msg::Marker::SPHERE_LIST; + m.color.r = 0.0; + m.color.g = 1.0; + m.color.b = 0.3; + m.color.a = 0.8; + m.scale.x = 0.05; + m.scale.y = 0.05; + m.scale.z = 0.05; + m.pose.orientation.w = 1.0; + + for (const auto & keyedPose : keyed_poses_) { + m.points.push_back(getPositionFromKey(keyedPose.first)); + } + graph_node_pub_->publish(m); + } +} diff --git a/kimera_graph_visualizer/src/visualizer_node.cpp b/kimera_graph_visualizer/src/visualizer_node.cpp new file mode 100644 index 0000000..cbcae5c --- /dev/null +++ b/kimera_graph_visualizer/src/visualizer_node.cpp @@ -0,0 +1,10 @@ +#include "kimera_graph_visualizer/visualizer.hpp" + +int main(int argc, char ** argv) +{ + // Initiallize visualizer + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}