diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 8538179253480..e60a57631f0ed 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -35,4 +35,9 @@ + + + + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml index b04124d7e981e..195e817357e58 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml @@ -1,6 +1,6 @@ - + @@ -9,6 +9,9 @@ + + + @@ -37,14 +40,14 @@ - + @@ -72,10 +75,10 @@ - + @@ -97,7 +100,7 @@ - + - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/fix2pose.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/fix2pose.launch.xml index ac92fce40cd6a..7627597305588 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/fix2pose.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/fix2pose.launch.xml @@ -3,14 +3,15 @@ - + + - + @@ -19,9 +20,9 @@ - + - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/septentrio_velocity_converter.launch.py b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/septentrio_velocity_converter.launch.py new file mode 100644 index 0000000000000..00608211408b4 --- /dev/null +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/septentrio_velocity_converter.launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='septentrio_velocity_converter', + executable='septentrio_velocity_converter_node', + name='septentrio_velocity_converter_node', + output='screen', + parameters=[ + {'coordinate_system': 'Cartesian'} # Geodetic or Cartesian + ] + ) + ]) \ No newline at end of file diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml index e4b04d3d4a32e..574a5a8b37bbf 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ndt_scan_matcher.launch.xml @@ -1,5 +1,6 @@ + @@ -7,7 +8,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index abbb15c797d19..eb8dc6732b7bd 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -7,7 +7,9 @@ - + + + @@ -135,4 +137,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_localization_launch/launch/switching.launch.xml b/launch/tier4_localization_launch/launch/switching.launch.xml new file mode 100644 index 0000000000000..d45f8467aeb5a --- /dev/null +++ b/launch/tier4_localization_launch/launch/switching.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file 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 53794fb93b7c6..6a335c94264b9 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 @@ -118,6 +118,10 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); lanelet::ConstPolygons3d obstacle_polygons = lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map); + lanelet::ConstPolygons3d gnss_available_area_polygon = + lanelet::utils::query::getAllGnssAvailableArea(viz_lanelet_map); + lanelet::ConstPolygons3d switching_area_polygon = + lanelet::utils::query::getAllSwitchingArea(viz_lanelet_map); lanelet::ConstPolygons3d no_obstacle_segmentation_area = lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area"); lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out = @@ -133,7 +137,7 @@ void Lanelet2MapVisualizationNode::onMapBin( cl_speed_bumps, cl_crosswalks, 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_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas; + cl_hatched_road_markings_line, cl_no_parking_areas, cl_gnss_available_area, cl_switching_area; 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); @@ -156,6 +160,8 @@ void Lanelet2MapVisualizationNode::onMapBin( 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); setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + setColor(&cl_gnss_available_area, 0.4, 0.27, 0.27, 0.5); + setColor(&cl_switching_area, 0.27, 0.27, 0.4, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; @@ -242,6 +248,14 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::gnssavailableareaPolygonsAsMarkerArray(gnss_available_area_polygon, cl_gnss_available_area)); + + insertMarkerArray( + &map_marker_array, + lanelet::visualization::switchingareaPolygonsAsMarkerArray(switching_area_polygon, cl_switching_area)); + pub_marker_->publish(map_marker_array); }