TEMP COMMIT

tmp launch

(tmp) change perception header times

tmp perception

(tmp) remove protection in tracking_object_merger

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

tmp motion_vel_smoother header update

tmp scenario_selector header update

tmp planning_validator header update

tmp vehicle cmd gate fix header time

Author: brkay54

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -454,6 +454,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   vehicle_cmd_emergency.stamp = filtered_commands.control.stamp;
 
   // Publish commands
+  filtered_commands.control.stamp = this->now();
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
   control_cmd_pub_->publish(filtered_commands.control);
   adapi_pause_->publish();

launch/tier4_simulator_launch/launch/simulator.launch.xml

@@ -32,15 +32,6 @@
     </include>
   </group>
 
-  <!-- Dummy Perception -->
-  <group if="$(var launch_dummy_perception)">
-    <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>
   <group unless="$(var scenario_simulation)">
     <!-- Occupancy Grid -->
     <push-ros-namespace namespace="occupancy_grid_map"/>
@@ -56,65 +47,6 @@
     </include>
   </group>
 
-  <!-- perception module -->
-  <group>
-    <push-ros-namespace namespace="perception"/>
-    <!-- object recognition -->
-    <group if="$(var perception/enable_object_recognition)">
-      <push-ros-namespace namespace="object_recognition"/>
-      <!-- tracking module -->
-      <group>
-        <push-ros-namespace namespace="tracking"/>
-        <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/tracking/tracking.launch.xml">
-          <arg
-            name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"
-            value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"
-          />
-          <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
-          <arg name="object_recognition_tracking_radar_object_tracker_node_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
-          <arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
-          <arg name="object_recognition_tracking_object_merger_node_param_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
-        </include>
-      </group>
-      <!-- prediction module -->
-      <group>
-        <push-ros-namespace namespace="prediction"/>
-        <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/prediction/prediction.launch.xml">
-          <arg name="use_vector_map" value="true"/>
-        </include>
-      </group>
-    </group>
-
-    <!-- publish empty objects instead of object recognition module -->
-    <group unless="$(var perception/enable_object_recognition)">
-      <push-ros-namespace namespace="object_recognition"/>
-      <node pkg="dummy_perception_publisher" exec="empty_objects_publisher" name="empty_objects_publisher" output="screen">
-        <remap from="~/output/objects" to="/perception/object_recognition/objects"/>
-      </node>
-    </group>
-
-    <group if="$(var perception/enable_elevation_map)">
-      <push-ros-namespace namespace="obstacle_segmentation/elevation_map"/>
-      <node pkg="elevation_map_loader" exec="elevation_map_loader" name="elevation_map_loader" output="screen">
-        <remap from="output/elevation_map" to="map"/>
-        <remap from="input/pointcloud_map" to="/map/pointcloud_map"/>
-        <remap from="input/vector_map" to="/map/vector_map"/>
-        <param name="use_lane_filter" value="false"/>
-        <param name="use_inpaint" value="true"/>
-        <param name="inpaint_radius" value="1.0"/>
-        <param name="param_file_path" value="$(var obstacle_segmentation_ground_segmentation_elevation_map_param_path)"/>
-        <param name="elevation_map_directory" value="$(find-pkg-share elevation_map_loader)/data/elevation_maps"/>
-        <param name="use_elevation_map_cloud_publisher" value="false"/>
-      </node>
-    </group>
-
-    <!-- traffic light module -->
-    <group if="$(var perception/enable_traffic_light)">
-      <push-ros-namespace namespace="traffic_light_recognition"/>
-      <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml"/>
-    </group>
-  </group>
-
   <!-- Dummy localization -->
   <group if="$(var launch_dummy_localization)">
     <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">

perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp

@@ -30,6 +30,7 @@ void DetectedObjectFeatureRemover::objectCallback(
 {
   DetectedObjects output;
   convert(*input, output);
+  output.header.stamp = this->now();
   pub_->publish(output);
 }
 

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -118,6 +118,7 @@ void ObjectLaneletFilterNode::objectCallback(
     }
     ++index;
   }
+  output_object_msg.header.stamp = this->now();
   object_pub_->publish(output_object_msg);
 
   // Publish debug info

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -345,7 +345,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
       removed_objects.objects.push_back(object);
     }
   }
-
+  output.header.stamp = this->now();
   objects_pub_->publish(output);
   if (debugger_) {
     debugger_->publishRemovedObjects(removed_objects);

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -219,6 +219,7 @@ void DetectionByTracker::onObjects(
       !available_trackers ||
       !object_recognition_utils::transformObjects(
         objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) {
+      detected_objects.header.stamp = this->now();
       objects_pub_->publish(detected_objects);
       return;
     }
@@ -248,7 +249,7 @@ void DetectionByTracker::onObjects(
   for (const auto & divided_object : divided_objects.feature_objects) {
     detected_objects.objects.push_back(divided_object.object);
   }
-
+  detected_objects.header.stamp = this->now();
   objects_pub_->publish(detected_objects);
 }
 

perception/lidar_centerpoint/src/node.cpp

@@ -160,7 +160,7 @@ void LidarCenterPointNode::pointCloudCallback(
   output_msg.objects = iou_bev_nms_.apply(raw_objects);
 
   detection_class_remapper_.mapClasses(output_msg);
-
+  output_msg.header.stamp = this->now();
   if (objects_sub_count > 0) {
     objects_pub_->publish(output_msg);
   }

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -1112,7 +1112,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
       }
     }
   }
-
+  output.header.stamp = this->now();
   // Publish Results
   pub_objects_->publish(output);
   pub_debug_markers_->publish(debug_markers);

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

@@ -304,6 +304,7 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
           ogm_frame_filtered_pc, *tf2_, base_link_frame_, *base_link_frame_filtered_pc_ptr)) {
       return;
     }
+    base_link_frame_filtered_pc_ptr->header.stamp = this->now();
     pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr));
   }
   if (debugger_ptr_) {

perception/tracking_object_merger/src/decorative_tracker_merger.cpp

@@ -250,9 +250,9 @@ bool DecorativeTrackerMergerNode::decorativeMerger(
 {
   // get current time
   const auto current_time = rclcpp::Time(input_objects_msg->header.stamp);
-  if (input_objects_msg->objects.empty()) {
-    return false;
-  }
+//  if (input_objects_msg->objects.empty()) {
+//    return false;
+//  }
   if (inner_tracker_objects_.empty()) {
     for (const auto & object : input_objects_msg->objects) {
       inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object));

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -312,7 +312,8 @@ void MotionVelocitySmootherNode::initCommonParam()
 void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
 {
   Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory);
-  publishing_trajectory.header = base_traj_raw_ptr_->header;
+  publishing_trajectory.header.frame_id = base_traj_raw_ptr_->header.frame_id;
+  publishing_trajectory.header.stamp = this->now();
   pub_trajectory_->publish(publishing_trajectory);
 }
 

planning/planning_validator/src/planning_validator.cpp

@@ -196,8 +196,10 @@ void PlanningValidator::publishTrajectory()
 {
   // Validation check is all green. Publish the trajectory.
   if (isAllValid(validation_status_)) {
-    pub_traj_->publish(*current_trajectory_);
-    previous_published_trajectory_ = current_trajectory_;
+    auto msg_sent = *current_trajectory_;
+    msg_sent.header.stamp = this->now();
+    pub_traj_->publish(msg_sent);
+    previous_published_trajectory_ = std::make_shared<Trajectory>(msg_sent);
     return;
   }
 

planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp

@@ -316,16 +316,18 @@ void ScenarioSelectorNode::onParkingTrajectory(
 void ScenarioSelectorNode::publishTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg)
 {
-  const auto now = this->now();
-  const auto delay_sec = (now - msg->header.stamp).seconds();
-  if (delay_sec <= th_max_message_delay_sec_) {
-    pub_trajectory_->publish(*msg);
-  } else {
-    RCLCPP_WARN_THROTTLE(
-      this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
-      "trajectory is delayed: scenario = %s, delay = %f, th_max_message_delay = %f",
-      current_scenario_.c_str(), delay_sec, th_max_message_delay_sec_);
-  }
+  auto msg_sent = *msg;
+  msg_sent.header.stamp = this->now();
+  pub_trajectory_->publish(msg_sent);
+
+  //  const auto delay_sec = (now - msg->header.stamp).seconds();
+  //  if (delay_sec <= th_max_message_delay_sec_) {
+  //  } else {
+  //    RCLCPP_WARN_THROTTLE(
+  //      this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
+  //      "trajectory is delayed: scenario = %s, delay = %f, th_max_message_delay = %f",
+  //      current_scenario_.c_str(), delay_sec, th_max_message_delay_sec_);
+  //  }
 }
 
 ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_options)