fix(dummy_perception_publisher): add parameter to configure z pose of…

… dummy object (autowarefoundation#3457)

Signed-off-by: Berkay Karaman <[email protected]>

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -16,6 +16,7 @@
   <arg name="perception/enable_object_recognition" default="false" description="enable object recognition"/>
   <arg name="perception/enable_elevation_map" default="false" description="enable elevation map loader"/>
   <arg name="perception/enable_traffic_light" default="false" description="enable traffic light"/>
+  <arg name="perception/use_base_link_z" default="true" description="dummy perception uses base_link z axis coordinate if it is true"/>
   <arg name="sensing/visible_range" default="300.0" description="visible range when using dummy perception"/>
 
   <arg name="vehicle_model" description="vehicle model name"/>
@@ -34,6 +35,7 @@
     <include file="$(find-pkg-share dummy_perception_publisher)/launch/dummy_perception_publisher.launch.xml">
       <arg name="real" value="$(var perception/enable_detection_failure)"/>
       <arg name="use_object_recognition" value="$(var perception/enable_object_recognition)"/>
+      <arg name="use_base_link_z" value="$(var perception/use_base_link_z)"/>
       <arg name="visible_range" value="$(var sensing/visible_range)"/>
     </include>
   </group>

simulator/dummy_perception_publisher/README.md

@@ -24,12 +24,13 @@ This node publishes the result of the dummy detection with the type of perceptio
 
 ## Parameters
 
-| Name                        | Type   | Default Value | Explanation                                 |
-| --------------------------- | ------ | ------------- | ------------------------------------------- |
-| `visible_range`             | double | 100.0         | sensor visible range [m]                    |
-| `detection_successful_rate` | double | 0.8           | sensor detection rate. (min) 0.0 - 1.0(max) |
-| `enable_ray_tracing`        | bool   | true          | if True, use ray tracking                   |
-| `use_object_recognition`    | bool   | true          | if True, publish objects topic              |
+| Name                        | Type   | Default Value | Explanation                                      |
+| --------------------------- | ------ | ------------- | ------------------------------------------------ |
+| `visible_range`             | double | 100.0         | sensor visible range [m]                         |
+| `detection_successful_rate` | double | 0.8           | sensor detection rate. (min) 0.0 - 1.0(max)      |
+| `enable_ray_tracing`        | bool   | true          | if True, use ray tracking                        |
+| `use_object_recognition`    | bool   | true          | if True, publish objects topic                   |
+| `use_base_link_z`           | bool   | true          | if True, node uses z coordinate of ego base_link |
 
 ### Node Parameters
 

simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp

@@ -116,6 +116,7 @@ class DummyPerceptionPublisherNode : public rclcpp::Node
   bool enable_ray_tracing_;
   bool use_object_recognition_;
   bool use_real_param_;
+  bool use_base_link_z_;
   std::unique_ptr<PointCloudCreator> pointcloud_creator_;
 
   double angle_increment_;

simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml

@@ -4,6 +4,7 @@
   <arg name="visible_range" default="300.0"/>
   <arg name="real" default="true"/>
   <arg name="use_object_recognition" default="true"/>
+  <arg name="use_base_link_z" default="true"/>
 
   <group>
     <push-ros-namespace namespace="simulation"/>
@@ -19,6 +20,7 @@
         <param name="enable_ray_tracing" value="false"/>
         <param name="use_object_recognition" value="$(var use_object_recognition)"/>
         <param name="object_centric_pointcloud" value="false"/>
+        <param name="use_base_link_z" value="$(var use_base_link_z)"/>
       </node>
       <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
         <arg name="input/objects" value="/perception/object_recognition/detection/labeled_clusters"/>
@@ -36,6 +38,7 @@
         <param name="detection_successful_rate" value="1.0"/>
         <param name="enable_ray_tracing" value="false"/>
         <param name="use_object_recognition" value="$(var use_object_recognition)"/>
+        <param name="use_base_link_z" value="$(var use_base_link_z)"/>
       </node>
     </group>
 

simulator/dummy_perception_publisher/src/node.cpp

@@ -106,7 +106,7 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode()
   detection_successful_rate_ = this->declare_parameter("detection_successful_rate", 0.8);
   enable_ray_tracing_ = this->declare_parameter("enable_ray_tracing", true);
   use_object_recognition_ = this->declare_parameter("use_object_recognition", true);
-
+  use_base_link_z_ = this->declare_parameter("use_base_link_z", true);
   const bool object_centric_pointcloud =
     this->declare_parameter("object_centric_pointcloud", false);
 
@@ -295,15 +295,17 @@ void DummyPerceptionPublisherNode::objectCallback(
       tf2::toMsg(tf_map2object_origin, object.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));
-        object.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;
+      if (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));
+          object.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;
+        }
       }
 
       objects_.push_back(object);
@@ -339,18 +341,20 @@ 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;
+          if (use_base_link_z_) {
+            // 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;
         }
       }