diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 6e94294c0900f..fad15948c368f 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/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); } diff --git a/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp index 4ff96bf15257b..b486595a2cb7b 100644 --- a/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp +++ b/perception/tensorrt_yolo/lib/src/plugins/nms_plugin.cpp @@ -39,10 +39,10 @@ #include #include -#include #include #include +#include using nvinfer1::DataType; using nvinfer1::DimsExprs; @@ -62,14 +62,14 @@ const char * NMS_PLUGIN_NAMESPACE{""}; template void write(char *& buffer, const T & val) { - *reinterpret_cast(buffer) = val; + std::memcpy(buffer, &val, sizeof(T)); buffer += sizeof(T); } template void read(const char *& buffer, T & val) { - val = *reinterpret_cast(buffer); + std::memcpy(&val, buffer, sizeof(T)); buffer += sizeof(T); } diff --git a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp index 78edc568bb7c4..dc12921dc1e96 100644 --- a/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp +++ b/perception/tensorrt_yolo/lib/src/plugins/yolo_layer_plugin.cpp @@ -63,10 +63,10 @@ #include #include -#include #include #include +#include #include using nvinfer1::DataType; @@ -87,14 +87,14 @@ const char * YOLO_LAYER_PLUGIN_NAMESPACE{""}; template void write(char *& buffer, const T & val) { - *reinterpret_cast(buffer) = val; + std::memcpy(buffer, &val, sizeof(T)); buffer += sizeof(T); } template void read(const char *& buffer, T & val) { - val = *reinterpret_cast(buffer); + std::memcpy(&val, buffer, sizeof(T)); buffer += sizeof(T); } } // namespace diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 829134411e39b..f2a3610c7d56f 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/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(); diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index b8172f26e2e78..eb6f6a81095f8 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/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(&input->data[global_offset + x_offset]), - *reinterpret_cast(&input->data[global_offset + y_offset]), - *reinterpret_cast(&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(&output.data[output_size + x_offset]) = point[0]; - *reinterpret_cast(&output.data[output_size + y_offset]) = point[1]; - *reinterpret_cast(&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; diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/system_monitor/reader/hdd_reader/hdd_reader.cpp index 9d8b54de0773e..6053c183eda4d 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/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(&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(&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;