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/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 8180a24..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(pose_graph_tools) - -find_package(catkin_simple REQUIRED) - -catkin_simple() - -cs_add_library(${PROJECT_NAME} - src/visualizer.cpp -) - -cs_add_executable(visualizer_node - src/visualizer_node.cpp - src/visualizer.cpp -) - -cs_install() -cs_export() diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..92393c6 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/include/pose_graph_tools/visualizer.h b/include/pose_graph_tools/visualizer.h deleted file mode 100644 index e56d7b7..0000000 --- a/include/pose_graph_tools/visualizer.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef VISUALIZER_H -#define VISUALIZER_H - -#include - -#include -#include - -#include - -#include - -#include - -class Visualizer { -public: - Visualizer(ros::NodeHandle& nh); - - void visualize(); - -private: - void PoseGraphCallback(const pose_graph_tools::PoseGraph::ConstPtr& msg); - - geometry_msgs::Point getPositionFromKey(long unsigned int key) const; - - void MakeMenuMarker(const tf::Pose &position, const std::string &id_number); - -private: - std::string frame_id_; - - // subscribers - ros::Subscriber pose_graph_sub_; - - // publishers - ros::Publisher odometry_edge_pub_; - ros::Publisher loop_edge_pub_; - ros::Publisher rejected_loop_edge_pub_; - ros::Publisher graph_node_pub_; - ros::Publisher 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 \ No newline at end of file diff --git a/msg/PoseGraph.msg b/msg/PoseGraph.msg deleted file mode 100644 index f96e06d..0000000 --- a/msg/PoseGraph.msg +++ /dev/null @@ -1,5 +0,0 @@ -Header header - -# Nodes and edges -PoseGraphNode[] nodes -PoseGraphEdge[] edges \ No newline at end of file diff --git a/msg/PoseGraphNode.msg b/msg/PoseGraphNode.msg deleted file mode 100644 index 08f18b1..0000000 --- a/msg/PoseGraphNode.msg +++ /dev/null @@ -1,5 +0,0 @@ -Header header - -uint64 key - -geometry_msgs/Pose pose \ No newline at end of file diff --git a/package.xml b/package.xml deleted file mode 100644 index ce3fd2f..0000000 --- a/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - pose_graph_tools - 1.0.0 - The pose_graph_tools package - - Yun Chang - - BSD - - catkin - catkin_simple - - roscpp - message_generation - message_runtime - std_msgs - geometry_msgs - visualization_msgs - interactive_markers - diff --git a/pose_graph_msgs/CMakeLists.txt b/pose_graph_msgs/CMakeLists.txt new file mode 100644 index 0000000..fd267ac --- /dev/null +++ b/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() diff --git a/pose_graph_msgs/msg/PoseGraph.msg b/pose_graph_msgs/msg/PoseGraph.msg new file mode 100644 index 0000000..bea8cfc --- /dev/null +++ b/pose_graph_msgs/msg/PoseGraph.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +# Nodes and edges +PoseGraphNode[] nodes +PoseGraphEdge[] edges diff --git a/msg/PoseGraphEdge.msg b/pose_graph_msgs/msg/PoseGraphEdge.msg similarity index 80% rename from msg/PoseGraphEdge.msg rename to pose_graph_msgs/msg/PoseGraphEdge.msg index 110bb3a..5b275ff 100644 --- a/msg/PoseGraphEdge.msg +++ b/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 \ No newline at end of file +float64[36] covariance diff --git a/pose_graph_msgs/msg/PoseGraphNode.msg b/pose_graph_msgs/msg/PoseGraphNode.msg new file mode 100644 index 0000000..1d55100 --- /dev/null +++ b/pose_graph_msgs/msg/PoseGraphNode.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +uint64 key + +geometry_msgs/Pose pose diff --git a/pose_graph_msgs/package.xml b/pose_graph_msgs/package.xml new file mode 100644 index 0000000..146e4b4 --- /dev/null +++ b/pose_graph_msgs/package.xml @@ -0,0 +1,28 @@ + + + + pose_graph_msgs + 1.0.0 + The pose_graph_msgs package + + Yun Chang + + BSD + + ament_cmake + + std_msgs + geometry_msgs + + builtin_interfaces + rosidl_default_generators + + builtin_interfaces + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/pose_graph_visualizer/CMakeLists.txt b/pose_graph_visualizer/CMakeLists.txt new file mode 100644 index 0000000..54dcf1f --- /dev/null +++ b/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() diff --git a/pose_graph_visualizer/include/pose_graph_visualizer/visualizer.hpp b/pose_graph_visualizer/include/pose_graph_visualizer/visualizer.hpp new file mode 100644 index 0000000..aa63c16 --- /dev/null +++ b/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 + +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::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/launch/posegraph_view.launch b/pose_graph_visualizer/launch/posegraph_view.launch.xml similarity index 80% rename from launch/posegraph_view.launch rename to pose_graph_visualizer/launch/posegraph_view.launch.xml index d952bdf..537f33a 100644 --- a/launch/posegraph_view.launch +++ b/pose_graph_visualizer/launch/posegraph_view.launch.xml @@ -4,7 +4,7 @@ - diff --git a/pose_graph_visualizer/package.xml b/pose_graph_visualizer/package.xml new file mode 100644 index 0000000..ab6454e --- /dev/null +++ b/pose_graph_visualizer/package.xml @@ -0,0 +1,23 @@ + + + + pose_graph_visualizer + 1.0.0 + The pose_graph_visualizer package + + Yun Chang + + BSD + + ament_cmake + + rclcpp + geometry_msgs + visualization_msgs + pose_graph_msgs + interactive_markers + + + ament_cmake + + diff --git a/pose_graph_visualizer/src/visualizer.cpp b/pose_graph_visualizer/src/visualizer.cpp new file mode 100644 index 0000000..7c50fba --- /dev/null +++ b/pose_graph_visualizer/src/visualizer.cpp @@ -0,0 +1,238 @@ +#include "pose_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 pose_graph_msgs::msg::PoseGraph::SharedPtr msg) { + // iterate through nodes in pose graph + for (const pose_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 pose_graph_msgs::msg::PoseGraphEdge &msg_edge : msg->edges) { + if (msg_edge.type == pose_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 == pose_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 == pose_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/pose_graph_visualizer/src/visualizer_node.cpp b/pose_graph_visualizer/src/visualizer_node.cpp new file mode 100644 index 0000000..8fcd70c --- /dev/null +++ b/pose_graph_visualizer/src/visualizer_node.cpp @@ -0,0 +1,9 @@ +#include "pose_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; +} diff --git a/src/visualizer.cpp b/src/visualizer.cpp deleted file mode 100644 index a0bb744..0000000 --- a/src/visualizer.cpp +++ /dev/null @@ -1,244 +0,0 @@ -#include -#include -#include -#include -#include - -Visualizer::Visualizer(ros::NodeHandle& nh) { - ROS_INFO("Initializing pose graph visualizer"); - - // get parameters - if (!nh.getParam("frame_id", frame_id_)) { - ROS_ERROR("Did not load frame id"); - } - - // start subscribers - pose_graph_sub_ = nh.subscribe( - "graph", 10, &Visualizer::PoseGraphCallback, this); - - // start publishers - odometry_edge_pub_ = nh.advertise( - "odometry_edges", 10, false); - loop_edge_pub_ = nh.advertise( - "loop_edges", 10, false); - rejected_loop_edge_pub_ = nh.advertise( - "rejected_loop_edges", 10, false); - - graph_node_pub_ = nh.advertise( - "graph_nodes", 10, false); - graph_node_id_pub_ = nh.advertise( - "graph_nodes_ids", 10, false); - - interactive_mrkr_srvr_ = std::make_shared( - "interactive_node", "", false); - - ros::spin(); -} - -void Visualizer::PoseGraphCallback( - const pose_graph_tools::PoseGraph::ConstPtr& msg) { - // iterate through nodes in pose graph - for (const pose_graph_tools::PoseGraphNode &msg_node : msg->nodes) { - tf::Pose pose; - tf::poseMsgToTF(msg_node.pose, pose); - - // Fill pose nodes (representing the robot position) - keyed_poses_[msg_node.key] = pose; - } - - // iterate through edges in pose graph - for (const pose_graph_tools::PoseGraphEdge &msg_edge : msg->edges) { - if (msg_edge.type == pose_graph_tools::PoseGraphEdge::ODOM) { - odometry_edges_.emplace_back( - std::make_pair(msg_edge.key_from, msg_edge.key_to)); - } else if (msg_edge.type == pose_graph_tools::PoseGraphEdge::LOOPCLOSE) { - loop_edges_.emplace_back( - std::make_pair(msg_edge.key_from, msg_edge.key_to)); - } else if (msg_edge.type == pose_graph_tools::PoseGraphEdge::REJECTED_LOOPCLOSE) { - rejected_loop_edges_.emplace_back( - std::make_pair(msg_edge.key_from, msg_edge.key_to)); - } - } - - visualize(); -} - -geometry_msgs::Point Visualizer::getPositionFromKey( - long unsigned int key) const { - tf::Vector3 v = keyed_poses_.at(key).getOrigin(); - geometry_msgs::Point p; - p.x = v.x(); - p.y = v.y(); - p.z = v.z(); - return p; -} - -// Interactive Marker Menu to click and see key of node -void Visualizer::MakeMenuMarker(const tf::Pose &position, - const std::string &id_number) { - interactive_markers::MenuHandler menu_handler; - - visualization_msgs::InteractiveMarker int_marker; - int_marker.header.frame_id = frame_id_; - int_marker.scale = 1.0; - tf::poseTFToMsg(position, int_marker.pose); - int_marker.name = id_number; - - visualization_msgs::Marker marker; - marker.type = visualization_msgs::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::InteractiveMarkerControl control; - control.interaction_mode = visualization_msgs::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_.getNumSubscribers() > 0) { - visualization_msgs::Marker m; - m.header.frame_id = frame_id_; - m.ns = frame_id_; - m.id = 0; - m.action = visualization_msgs::Marker::ADD; - m.type = visualization_msgs::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_.getNumSubscribers() > 0) { - visualization_msgs::Marker m; - m.header.frame_id = frame_id_; - m.ns = frame_id_; - m.id = 1; - m.action = visualization_msgs::Marker::ADD; - m.type = visualization_msgs::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_.getNumSubscribers() > 0) { - visualization_msgs::Marker m; - m.header.frame_id = frame_id_; - m.ns = frame_id_; - m.id = 1; - m.action = visualization_msgs::Marker::ADD; - m.type = visualization_msgs::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_.getNumSubscribers() > 0) { - visualization_msgs::Marker m; - m.header.frame_id = frame_id_; - m.ns = frame_id_; - - m.action = visualization_msgs::Marker::ADD; - m.type = visualization_msgs::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; - int counter = 0; - for (const auto &keyedPose : keyed_poses_) { - tf::poseTFToMsg(keyedPose.second, m.pose); - // 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_.getNumSubscribers() > 0) { - visualization_msgs::Marker m; - m.header.frame_id = frame_id_; - m.ns = frame_id_; - m.id = 4; - m.action = visualization_msgs::Marker::ADD; - m.type = visualization_msgs::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/src/visualizer_node.cpp b/src/visualizer_node.cpp deleted file mode 100644 index 53e1e61..0000000 --- a/src/visualizer_node.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include -#include -#include - -int main(int argc, char *argv[]) { - // Initiallize visualizer - ros::init(argc, argv, "visualizer"); - ros::NodeHandle nh; - Visualizer viz(nh); -} \ No newline at end of file