fix(dummy_perception_publisher): modified objects also use baselink z…

…-position (#588)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

simulator/dummy_perception_publisher/src/node.cpp

@@ -402,6 +402,18 @@ void DummyPerceptionPublisherNode::objectCallback(
           dummy_perception_publisher::msg::Object object;
           objects_.at(i) = *msg;
           tf2::toMsg(tf_map2object_origin, objects_.at(i).initial_state.pose_covariance.pose);
+
+          // Use base_link Z
+          geometry_msgs::msg::TransformStamped ros_map2base_link;
+          try {
+            ros_map2base_link = tf_buffer_.lookupTransform(
+              "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5));
+            objects_.at(i).initial_state.pose_covariance.pose.position.z =
+              ros_map2base_link.transform.translation.z;
+          } catch (tf2::TransformException & ex) {
+            RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what());
+            return;
+          }
           break;
         }
       }