Merge pull request #416 from tier4/sync-upstream

chore: sync upstream

map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp

@@ -121,12 +121,15 @@ void Lanelet2MapVisualizationNode::onMapBin(
   lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out =
     lanelet::utils::query::getAllPolygonsByType(
       viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out");
+  lanelet::ConstPolygons3d hatched_road_markings_area =
+    lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings");
 
   std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings,
     cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas,
     cl_speed_bumps, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons,
     cl_no_stopping_areas, cl_no_obstacle_segmentation_area,
-    cl_no_obstacle_segmentation_area_for_run_out;
+    cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area,
+    cl_hatched_road_markings_line;
   setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
   setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5);
@@ -145,6 +148,8 @@ void Lanelet2MapVisualizationNode::onMapBin(
   setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999);
   setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5);
   setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5);
+  setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5);
+  setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -219,6 +224,11 @@ void Lanelet2MapVisualizationNode::onMapBin(
     lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray(
       no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out));
 
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray(
+      hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line));
+
   pub_marker_->publish(map_marker_array);
 }
 

perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp

@@ -39,10 +39,10 @@
 
 #include <cuda_runtime_api.h>
 #include <stdio.h>
-#include <string.h>
 
 #include <cassert>
 #include <cmath>
+#include <cstring>
 
 using nvinfer1::DataType;
 using nvinfer1::DimsExprs;
@@ -62,14 +62,14 @@ const char * NMS_PLUGIN_NAMESPACE{""};
 template <typename T>
 void write(char *& buffer, const T & val)
 {
-  *reinterpret_cast<T *>(buffer) = val;
+  std::memcpy(buffer, &val, sizeof(T));
   buffer += sizeof(T);
 }
 
 template <typename T>
 void read(const char *& buffer, T & val)
 {
-  val = *reinterpret_cast<const T *>(buffer);
+  std::memcpy(&val, buffer, sizeof(T));
   buffer += sizeof(T);
 }
 

perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp

@@ -63,10 +63,10 @@
 
 #include <cuda_runtime_api.h>
 #include <stdio.h>
-#include <string.h>
 
 #include <cassert>
 #include <cmath>
+#include <cstring>
 #include <vector>
 
 using nvinfer1::DataType;
@@ -87,14 +87,14 @@ const char * YOLO_LAYER_PLUGIN_NAMESPACE{""};
 template <typename T>
 void write(char *& buffer, const T & val)
 {
-  *reinterpret_cast<T *>(buffer) = val;
+  std::memcpy(buffer, &val, sizeof(T));
   buffer += sizeof(T);
 }
 
 template <typename T>
 void read(const char *& buffer, T & val)
 {
-  val = *reinterpret_cast<const T *>(buffer);
+  std::memcpy(&val, buffer, sizeof(T));
   buffer += sizeof(T);
 }
 }  // namespace

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -707,7 +707,13 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
 
   // set hazard and turn signal
   if (status_.has_decided_path) {
-    output.turn_signal_info = calcTurnSignalInfo();
+    const auto original_signal = getPreviousModuleOutput().turn_signal_info;
+    const auto new_signal = calcTurnSignalInfo();
+    const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
+    output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+      *output.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
+      planner_data_->parameters.ego_nearest_dist_threshold,
+      planner_data_->parameters.ego_nearest_yaw_threshold);
   }
 
   const auto distance_to_path_change = calcDistanceToPathChange();

sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp

@@ -127,10 +127,11 @@ void CropBoxFilterComponent::faster_filter(
 
   for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size();
        global_offset += input->point_step) {
-    Eigen::Vector4f point(
-      *reinterpret_cast<const float *>(&input->data[global_offset + x_offset]),
-      *reinterpret_cast<const float *>(&input->data[global_offset + y_offset]),
-      *reinterpret_cast<const float *>(&input->data[global_offset + z_offset]), 1);
+    Eigen::Vector4f point;
+    std::memcpy(&point[0], &input->data[global_offset + x_offset], sizeof(float));
+    std::memcpy(&point[1], &input->data[global_offset + y_offset], sizeof(float));
+    std::memcpy(&point[2], &input->data[global_offset + z_offset], sizeof(float));
+    point[3] = 1;
 
     if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || !std::isfinite(point[2])) {
       RCLCPP_WARN(this->get_logger(), "Ignoring point containing NaN values");
@@ -148,9 +149,9 @@ void CropBoxFilterComponent::faster_filter(
       memcpy(&output.data[output_size], &input->data[global_offset], input->point_step);
 
       if (transform_info.need_transform) {
-        *reinterpret_cast<float *>(&output.data[output_size + x_offset]) = point[0];
-        *reinterpret_cast<float *>(&output.data[output_size + y_offset]) = point[1];
-        *reinterpret_cast<float *>(&output.data[output_size + z_offset]) = point[2];
+        std::memcpy(&output.data[output_size + x_offset], &point[0], sizeof(float));
+        std::memcpy(&output.data[output_size + y_offset], &point[1], sizeof(float));
+        std::memcpy(&output.data[output_size + z_offset], &point[2], sizeof(float));
       }
 
       output_size += input->point_step;

system/system_monitor/reader/hdd_reader/hdd_reader.cpp

@@ -385,11 +385,11 @@ int get_nvme_smart_data(int fd, HddInfo * info)
   // is from 1 to 1,000, three indicates that the number of 512 byte data
   // units written is from 2,001 to 3,000)
   info->is_valid_total_data_written_ = true;
-  info->total_data_written_ = *(reinterpret_cast<uint64_t *>(&data[48]));
+  std::memcpy(&info->total_data_written_, &data[48], sizeof(info->total_data_written_));
 
   // Bytes 143:128 Power On Hours
   info->is_valid_power_on_hours_ = true;
-  info->power_on_hours_ = *(reinterpret_cast<uint64_t *>(&data[128]));
+  std::memcpy(&info->power_on_hours_, &data[128], sizeof(info->power_on_hours_));
 
   // NVMe S.M.A.R.T has no information of recovered error count
   info->is_valid_recovered_error_ = false;