From bc0a9781fbd50f1dbe9ab0cc4f47438b77f87d03 Mon Sep 17 00:00:00 2001 From: Hirokazu Ishida Date: Sun, 20 Mar 2022 06:35:28 +0900 Subject: [PATCH] [Refactor] create ObjectInfo struct --- .../dummy_perception_publisher/node.hpp | 14 ++++++++++ .../dummy_perception_publisher/src/node.cpp | 28 +++++++++++++++++++ 2 files changed, 42 insertions(+) diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index ceaea73c4c205..cd7c335617050 100644 --- 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 #include +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 index 856f092dabee1..239c2a50f8b60 100644 --- 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_) {