Merge pull request #8 from MFernandezCarmona/rolling

added use_header option
MOCAP4ROS2-Project · May 30, 2024 · 897699f · 897699f
2 parents 844fb7b + a462ed9
commit 897699f
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 16 deletions.
diff --git a/...2_marker_viz/mocap4r2_marker_viz/include/mocap4r2_marker_viz/mocap4r2_marker_viz_node.hpp b/...2_marker_viz/mocap4r2_marker_viz/include/mocap4r2_marker_viz/mocap4r2_marker_viz_node.hpp
@@ -52,10 +52,12 @@ class MarkerVisualizer : public rclcpp::Node
   void rb_callback(const mocap4r2_msgs::msg::RigidBodies::SharedPtr msg) const;
 
   visualization_msgs::msg::Marker marker2visual(
-    int index, const geometry_msgs::msg::Point & translation) const;
+    int index, const geometry_msgs::msg::Point & translation,
+    const std_msgs::msg::Header & header) const;
 
   visualization_msgs::msg::Marker rb2visual(
-    int index, const geometry_msgs::msg::Pose & poserb) const;
+    int index, const geometry_msgs::msg::Pose & poserb,
+    const std_msgs::msg::Header & header) const;
 
   geometry_msgs::msg::Pose mocap2rviz(const geometry_msgs::msg::Pose mocap4r2_pose) const;
 
@@ -67,7 +69,6 @@ class MarkerVisualizer : public rclcpp::Node
 
   geometry_msgs::msg::Vector3 marker_scale_;
   float marker_lifetime_;
-  std::string marker_frame_;
   std::string namespace_;
   std::string mocap4r2_system_;
   std_msgs::msg::ColorRGBA default_marker_color_;

diff --git a/mocap4r2_marker_viz/mocap4r2_marker_viz/src/mocap4r2_marker_viz_node.cpp b/mocap4r2_marker_viz/mocap4r2_marker_viz/src/mocap4r2_marker_viz_node.cpp
@@ -36,7 +36,6 @@ MarkerVisualizer::MarkerVisualizer()
   declare_parameter<double>("marker_scale_y", 0.014f);
   declare_parameter<double>("marker_scale_z", 0.014f);
   declare_parameter<float>("marker_lifetime", 0.01f);
-  declare_parameter<std::string>("marker_frame", "map");
   declare_parameter<std::string>("namespace", "mocap4r2_markers");
   declare_parameter<std::string>("mocap4r2_system", "optitrack");
 
@@ -48,7 +47,6 @@ MarkerVisualizer::MarkerVisualizer()
   get_parameter<double>("marker_scale_y", marker_scale_.y);
   get_parameter<double>("marker_scale_z", marker_scale_.z);
   get_parameter<float>("marker_lifetime", marker_lifetime_);
-  get_parameter<std::string>("marker_frame", marker_frame_);
   get_parameter<std::string>("namespace", namespace_);
   get_parameter<std::string>("mocap4r2_system", mocap4r2_system_);
 
@@ -100,17 +98,17 @@ MarkerVisualizer::marker_callback(const mocap4r2_msgs::msg::Markers::SharedPtr m
   static int counter = 0;
   visualization_msgs::msg::MarkerArray visual_markers;
   for (const mocap4r2_msgs::msg::Marker & marker : msg->markers) {
-    visual_markers.markers.push_back(marker2visual(counter++, marker.translation));
+    visual_markers.markers.push_back(marker2visual(counter++, marker.translation, msg->header));
   }
   publisher_->publish(visual_markers);
 }
 
 visualization_msgs::msg::Marker
-MarkerVisualizer::marker2visual(int index, const geometry_msgs::msg::Point & translation) const
+MarkerVisualizer::marker2visual(int index, const geometry_msgs::msg::Point & translation,
+                                const std_msgs::msg::Header  & header) const
 {
   visualization_msgs::msg::Marker viz_marker;
-  viz_marker.header.frame_id = marker_frame_;
-  viz_marker.header.stamp = rclcpp::Clock().now();
+  viz_marker.header = header;
   viz_marker.ns = namespace_;
   viz_marker.color = default_marker_color_;
   viz_marker.id = index;
@@ -125,7 +123,7 @@ MarkerVisualizer::marker2visual(int index, const geometry_msgs::msg::Point & tra
   marker_pose.orientation.w = 1.0f;
   viz_marker.pose = mocap2rviz(marker_pose);
   viz_marker.scale = marker_scale_;
-  viz_marker.lifetime = rclcpp::Duration(1s);
+  viz_marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime_);
   return viz_marker;
 }
 
@@ -142,10 +140,11 @@ MarkerVisualizer::rb_callback(const mocap4r2_msgs::msg::RigidBodies::SharedPtr m
   visualization_msgs::msg::MarkerArray visual_markers_rb;
 
   for (const mocap4r2_msgs::msg::RigidBody & rb : msg->rigidbodies) {
-    visual_markers_rb.markers.push_back(rb2visual(counter_rb++, rb.pose));
+    visual_markers_rb.markers.push_back(rb2visual(counter_rb++, rb.pose, msg->header));
 
     for (const mocap4r2_msgs::msg::Marker & marker : rb.markers) {
-      visual_markers_rb.markers.push_back(marker2visual(counter_markers_rb++, marker.translation));
+      visual_markers_rb.markers.push_back(marker2visual(counter_markers_rb++,
+                                                        marker.translation, msg->header));
     }
   }
 
@@ -154,11 +153,11 @@ MarkerVisualizer::rb_callback(const mocap4r2_msgs::msg::RigidBodies::SharedPtr m
 
 
 visualization_msgs::msg::Marker
-MarkerVisualizer::rb2visual(int index, const geometry_msgs::msg::Pose & poserb) const
+MarkerVisualizer::rb2visual(int index, const geometry_msgs::msg::Pose & poserb,
+                            const std_msgs::msg::Header  & header) const
 {
   visualization_msgs::msg::Marker viz_marker;
-  viz_marker.header.frame_id = marker_frame_;
-  viz_marker.header.stamp = rclcpp::Clock().now();
+  viz_marker.header = header;
   viz_marker.ns = namespace_;
   viz_marker.color = default_marker_color_;
   viz_marker.id = index;
@@ -173,6 +172,6 @@ MarkerVisualizer::rb2visual(int index, const geometry_msgs::msg::Pose & poserb)
   marker_scale_.y = 0.014f;
   marker_scale_.z = 0.014f;
   viz_marker.scale = marker_scale_;
-  viz_marker.lifetime = rclcpp::Duration(0.1s);
+  viz_marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime_);
   return viz_marker;
 }