[Refactor] create ObjectInfo struct

autowarefoundation · Mar 19, 2022 · bc0a978 · bc0a978
1 parent c268df8
commit bc0a978
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 0 deletions.
diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp
@@ -36,6 +36,20 @@
 #include <random>
 #include <vector>
 
+struct ObjectInfo
+{
+  ObjectInfo(
+    const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time);
+  double length;
+  double width;
+  double height;
+  double std_dev_x;
+  double std_dev_y;
+  double std_dev_z;
+  double std_dev_yaw;
+  tf2::Transform tf_map2moved_object;
+};
+
 class DummyPerceptionPublisherNode : public rclcpp::Node
 {
 private:

diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp
@@ -48,6 +48,34 @@ pcl::PointXYZ getBaseLinkToPoint(
   return point;
 };
 
+ObjectInfo::ObjectInfo(
+  const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time)
+: length(object.shape.dimensions.x),
+  width(object.shape.dimensions.y),
+  height(object.shape.dimensions.z),
+  std_dev_x(std::sqrt(object.initial_state.pose_covariance.covariance[0])),
+  std_dev_y(std::sqrt(object.initial_state.pose_covariance.covariance[7])),
+  std_dev_z(std::sqrt(object.initial_state.pose_covariance.covariance[14])),
+  std_dev_yaw(std::sqrt(object.initial_state.pose_covariance.covariance[35]))
+{
+  const double move_distance =
+    object.initial_state.twist_covariance.twist.linear.x *
+    (current_time.seconds() - rclcpp::Time(object.header.stamp).seconds());
+  tf2::Transform tf_object_origin2moved_object;
+  tf2::Transform tf_map2object_origin;
+  {
+    geometry_msgs::msg::Transform ros_object_origin2moved_object;
+    ros_object_origin2moved_object.translation.x = move_distance;
+    ros_object_origin2moved_object.rotation.x = 0;
+    ros_object_origin2moved_object.rotation.y = 0;
+    ros_object_origin2moved_object.rotation.z = 0;
+    ros_object_origin2moved_object.rotation.w = 1;
+    tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object);
+  }
+  tf2::fromMsg(object.initial_state.pose_covariance.pose, tf_map2object_origin);
+  this->tf_map2moved_object = tf_map2object_origin * tf_object_origin2moved_object;
+}
+
 DummyPerceptionPublisherNode::DummyPerceptionPublisherNode()
 : Node("dummy_perception_publisher"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
 {