Refactor and support for heading arrows

MIT-SPARK · Dec 20, 2024 · ad25c6a · ad25c6a
1 parent 9ddf339
commit ad25c6a
Show file tree

Hide file tree

Showing 3 changed files with 255 additions and 145 deletions.
diff --git a/pose_graph_tools_ros/include/pose_graph_tools_ros/visualizer.h b/pose_graph_tools_ros/include/pose_graph_tools_ros/visualizer.h
@@ -18,29 +18,23 @@ class Visualizer {
 
   void visualize();
 
+  typedef std::pair<int, uint64_t> Node;  // robot id, key
+  typedef std::pair<Node, Node> Edge;
+
  private:
   void PoseGraphCallback(const pose_graph_tools_msgs::PoseGraph::ConstPtr& msg);
 
   geometry_msgs::Point getPositionFromKey(int robot_id, uint64_t key) const;
 
-  void MakeMenuMarker(const geometry_msgs::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_;
+  ros::Publisher marker_array_pub_;
 
-  typedef std::pair<int, uint64_t> Node;  // robot id, key
-  typedef std::pair<Node, Node> Edge;
   std::vector<Edge> odometry_edges_;
   std::vector<Edge> loop_edges_;
   std::vector<Edge> rejected_loop_edges_;

diff --git a/pose_graph_tools_ros/src/conversions.cpp b/pose_graph_tools_ros/src/conversions.cpp
@@ -101,6 +101,7 @@ pose_graph_tools_msgs::PoseGraphNode toMsg(
   result.key = pose_graph_node.key;
   result.robot_id = pose_graph_node.robot_id;
   tf2::convert(pose_graph_node.pose, result.pose);
+
   return result;
 }