diff --git a/autoware.ai/autoware_files/carla_launch/1_sensing.launch b/autoware.ai/autoware_files/carla_launch/1_sensing.launch index 7877aaac..b7e35f73 100644 --- a/autoware.ai/autoware_files/carla_launch/1_sensing.launch +++ b/autoware.ai/autoware_files/carla_launch/1_sensing.launch @@ -21,9 +21,9 @@ - + - @@ -34,7 +34,7 @@ diff --git a/autoware.ai/autoware_files/carla_launch/2_localization_gicp.launch b/autoware.ai/autoware_files/carla_launch/2_localization_gicp.launch index dcd33289..5738da2e 100644 --- a/autoware.ai/autoware_files/carla_launch/2_localization_gicp.launch +++ b/autoware.ai/autoware_files/carla_launch/2_localization_gicp.launch @@ -25,7 +25,7 @@ - + diff --git a/autoware.ai/autoware_files/carla_launch/2_localization_param.launch b/autoware.ai/autoware_files/carla_launch/2_localization_param.launch index 89ca7b9c..f3b038fc 100644 --- a/autoware.ai/autoware_files/carla_launch/2_localization_param.launch +++ b/autoware.ai/autoware_files/carla_launch/2_localization_param.launch @@ -35,7 +35,7 @@ - + diff --git a/autoware.ai/autoware_files/carla_launch/4_planning.launch b/autoware.ai/autoware_files/carla_launch/4_planning.launch index f96e87fe..bdbe0f5b 100644 --- a/autoware.ai/autoware_files/carla_launch/4_planning.launch +++ b/autoware.ai/autoware_files/carla_launch/4_planning.launch @@ -20,7 +20,7 @@ - + diff --git a/autoware.ai/autoware_files/gnss_mapping.launch b/autoware.ai/autoware_files/gnss_mapping.launch index 6eec89a1..3c46f99c 100644 --- a/autoware.ai/autoware_files/gnss_mapping.launch +++ b/autoware.ai/autoware_files/gnss_mapping.launch @@ -33,8 +33,8 @@ - - + + diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/base/single_sensing.launch b/autoware.ai/autoware_files/lgsvl_file/launch/base/single_sensing.launch index aa5985b4..9c5b57e7 100644 --- a/autoware.ai/autoware_files/lgsvl_file/launch/base/single_sensing.launch +++ b/autoware.ai/autoware_files/lgsvl_file/launch/base/single_sensing.launch @@ -6,9 +6,9 @@ - + - + @@ -26,7 +26,7 @@ diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/desktop/single_sensing.launch b/autoware.ai/autoware_files/lgsvl_file/launch/desktop/single_sensing.launch index a431a452..d941a1f5 100644 --- a/autoware.ai/autoware_files/lgsvl_file/launch/desktop/single_sensing.launch +++ b/autoware.ai/autoware_files/lgsvl_file/launch/desktop/single_sensing.launch @@ -1,20 +1,20 @@ - + - + - + - + @@ -31,7 +31,7 @@ diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/lgsvl_bridge.launch b/autoware.ai/autoware_files/lgsvl_file/launch/lgsvl_bridge.launch index b4bd4276..eb15e97b 100644 --- a/autoware.ai/autoware_files/lgsvl_file/launch/lgsvl_bridge.launch +++ b/autoware.ai/autoware_files/lgsvl_file/launch/lgsvl_bridge.launch @@ -1,11 +1,11 @@ - + - + - + diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/map_loader.launch b/autoware.ai/autoware_files/lgsvl_file/launch/map_loader.launch index 2fe5748b..1fb35397 100644 --- a/autoware.ai/autoware_files/lgsvl_file/launch/map_loader.launch +++ b/autoware.ai/autoware_files/lgsvl_file/launch/map_loader.launch @@ -1,6 +1,6 @@ - + - diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/minicar/single_sensing.launch b/autoware.ai/autoware_files/lgsvl_file/launch/minicar/single_sensing.launch index 3fd2a96f..f6c44d32 100644 --- a/autoware.ai/autoware_files/lgsvl_file/launch/minicar/single_sensing.launch +++ b/autoware.ai/autoware_files/lgsvl_file/launch/minicar/single_sensing.launch @@ -1,19 +1,19 @@ - + - + - + - + @@ -30,7 +30,7 @@ diff --git a/autoware.ai/autoware_files/lgsvl_file/launch/nvidia/single_sensing.launch b/autoware.ai/autoware_files/lgsvl_file/launch/nvidia/single_sensing.launch index 797d0f89..3c680ac1 100644 --- a/autoware.ai/autoware_files/lgsvl_file/launch/nvidia/single_sensing.launch +++ b/autoware.ai/autoware_files/lgsvl_file/launch/nvidia/single_sensing.launch @@ -1,20 +1,20 @@ - + - + - + - + @@ -31,7 +31,7 @@ diff --git a/autoware.ai/autoware_files/rviz/cubetown.rviz b/autoware.ai/autoware_files/rviz/cubetown.rviz new file mode 100644 index 00000000..c01a7761 --- /dev/null +++ b/autoware.ai/autoware_files/rviz/cubetown.rviz @@ -0,0 +1,719 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Grid1 + - /System1 + - /System1/TF1 + - /System1/TF1/Frames1 + - /Map1 + - /Map1/Vector Map1 + - /Perception1 + - /Perception1/Tracked Objects1 + - /Planning1 + - /Planning1/Saved Waypoints1 + Splitter Ratio: 0.642276406288147 + Tree Height: 1042 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Points Map + - Class: autoware_rviz_debug/DecisionMakerPanel + Name: DecisionMakerPanel + - Class: integrated_viewer/ImageViewerPlugin + Image topic: /image_raw + Lane topic: ----- + Name: ImageViewerPlugin + Point size: 3 + Point topic: ----- + Rect topic: /detection/image_detector/objects +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 10 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: base_link + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + camera: + Value: false + map: + Value: true + mobility: + Value: false + velodyne: + Value: true + world: + Value: false + Marker Scale: 5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + map: + base_link: + velodyne: + camera: + {} + mobility: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: Vehicle Model + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Group + Displays: + - Buffer length: 100 + Class: jsk_rviz_plugin/Plotter2D + Enabled: true + Name: Velocity (km/h) + Show Value: true + Topic: /linear_velocity_viz + Value: true + auto color change: false + auto scale: true + background color: 0; 0; 0 + backround alpha: 0 + border: true + foreground alpha: 1 + foreground color: 0; 255; 255 + height: 80 + left: 40 + linewidth: 1 + max color: 255; 0; 0 + max value: 1 + min value: -1 + show caption: true + text size: 8 + top: 30 + update interval: 0.03999999910593033 + width: 80 + - Buffer length: 100 + Class: jsk_rviz_plugin/Plotter2D + Enabled: true + Name: NDT Time [ms] + Show Value: true + Topic: /time_ndt_matching + Value: true + auto color change: false + auto scale: true + background color: 0; 0; 0 + backround alpha: 0 + border: true + foreground alpha: 1 + foreground color: 0; 255; 255 + height: 80 + left: 140 + linewidth: 1 + max color: 255; 0; 0 + max value: 1 + min value: -1 + show caption: true + text size: 8 + top: 30 + update interval: 0.03999999910593033 + width: 80 + - Align Bottom: false + Background Alpha: 0.8999999761581421 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000011920929 + Foreground Color: 0; 255; 255 + Invert Shadow: false + Name: NDT Monitor + Overtake BG Color Properties: false + Overtake FG Color Properties: false + Overtake Position Properties: true + Topic: /ndt_monitor/ndt_info_text + Value: true + font: DejaVu Sans Mono + height: 50 + left: 40 + line width: 2 + text size: 8 + top: 150 + width: 200 + - Align Bottom: false + Background Alpha: 0.8999999761581421 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000011920929 + Foreground Color: 0; 255; 255 + Invert Shadow: false + Name: Decision Maker Panel + Overtake BG Color Properties: false + Overtake FG Color Properties: false + Overtake Position Properties: true + Topic: /decision_maker/state_overlay + Value: true + font: DejaVu Sans Mono + height: 200 + left: 40 + line width: 2 + text size: 8 + top: 220 + width: 200 + Enabled: true + Name: Monitor + - Class: rviz/Group + Displays: + - Align Bottom: false + Background Alpha: 0.800000011920929 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000011920929 + Foreground Color: 25; 255; 240 + Invert Shadow: false + Name: OK + Overtake BG Color Properties: false + Overtake FG Color Properties: false + Overtake Position Properties: true + Topic: /health_aggregator/ok_text + Value: true + font: DejaVu Sans Mono + height: 80 + left: 40 + line width: 2 + text size: 6 + top: 430 + width: 200 + - Align Bottom: false + Background Alpha: 0.800000011920929 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000011920929 + Foreground Color: 255; 255; 0 + Invert Shadow: false + Name: WARN + Overtake BG Color Properties: false + Overtake FG Color Properties: false + Overtake Position Properties: true + Topic: /health_aggregator/warn_text + Value: true + font: DejaVu Sans Mono + height: 80 + left: 40 + line width: 2 + text size: 6 + top: 520 + width: 200 + - Align Bottom: false + Background Alpha: 0.800000011920929 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000011920929 + Foreground Color: 255; 85; 0 + Invert Shadow: false + Name: ERROR + Overtake BG Color Properties: false + Overtake FG Color Properties: false + Overtake Position Properties: true + Topic: /health_aggregator/error_text + Value: true + font: DejaVu Sans Mono + height: 80 + left: 40 + line width: 2 + text size: 6 + top: 620 + width: 200 + - Align Bottom: false + Background Alpha: 0.800000011920929 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.800000011920929 + Foreground Color: 255; 0; 0 + Invert Shadow: false + Name: FATAL + Overtake BG Color Properties: false + Overtake FG Color Properties: false + Overtake Position Properties: true + Topic: /health_aggregator/fatal_text + Value: true + font: DejaVu Sans Mono + height: 80 + left: 40 + line width: 2 + text size: 6 + top: 720 + width: 200 + Enabled: true + Name: Health Checker + Enabled: true + Name: System + - Class: rviz/Group + Displays: + - Alpha: 0.05000000074505806 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Points Map + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /points_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /vector_map_center_lines_rviz + Name: Vector Map + Namespaces: + road_network_vector_map: true + Queue Size: 100 + Value: true + Enabled: true + Name: Map + - Class: rviz/Group + Displays: + - Alpha: 0.30000001192092896 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 23.83440399169922 + Min Value: -22.532455444335938 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Points Raw + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /points_raw + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 23.83440399169922 + Min Value: -22.532455444335938 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Points No Ground + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /points_no_ground + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Sensing + - Class: rviz/Group + Displays: + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 170; 255 + Enabled: false + Head Length: 2 + Head Radius: 2 + Name: Current Pose + Shaft Length: 2 + Shaft Radius: 1 + Shape: Arrow + Topic: /current_pose + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 255; 0 + Enabled: false + Head Length: 2 + Head Radius: 1 + Name: EKF Pose + Shaft Length: 2 + Shaft Radius: 0.5 + Shape: Arrow + Topic: /ekf_pose + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 0; 255; 255 + Enabled: false + Head Length: 2 + Head Radius: 1 + Name: NDT Pose + Shaft Length: 2 + Shaft Radius: 0.5 + Shape: Arrow + Topic: /ndt_pose + Unreliable: false + Value: false + Enabled: true + Name: Localization + - Class: rviz/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Occupancy Grid Map + Topic: /semantics/costmap_generator/occupancy_grid + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 23.83440399169922 + Min Value: -22.532455444335938 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Clustered Points + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /points_cluster + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /detection/lidar_detector/objects_markers_center + Name: Tracked Objects + Namespaces: + /detection/lidar_detector/centroid_markers2: true + /detection/lidar_detector/hull_markers2: true + /detection/lidar_detector/label_markers2: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /prediction/motion_predictor/path_markers + Name: Predicted Path + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: Perception + - Class: rviz/Group + Displays: + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /local_trajectories_eval_rviz + Name: Saved Waypoints + Namespaces: + local_lane_array_marker_colored: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /global_waypoints_mark + Name: Global Waypoints + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /local_waypoints_mark + Name: Local Waypoints + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /detection_range + Name: Detection Range + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /safety_border + Name: Safety Box + Namespaces: + global_lane_array_marker: true + Queue Size: 100 + Value: true + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: Simulated Obstacle + Topic: /dp_planner_tracked_boxes + Unreliable: false + Value: true + alpha: 0.800000011920929 + color: 25; 255; 0 + coloring: Value + line width: 0.009999999776482582 + only edge: false + show coords: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /AnimateGlobalPlan + Name: GlobalPathAnimation + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /behavior_state + Name: Behavior State + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /detected_polygons + Name: Tracked Contours + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /local_trajectories + Name: Local Rollouts + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /global_waypoints_rviz + Name: Global Path + Namespaces: + global_lane_array_marker: true + global_lane_waypoint_orientation_marker: true + Queue Size: 100 + Value: true + Enabled: true + Name: OP Planner + Enabled: true + Name: Planning + - Class: rviz/Group + Displays: + - Class: rviz/Marker + Enabled: true + Marker Topic: /trajectory_circle_mark + Name: Pure Pursuit Trajectory + Namespaces: + trajectory_circle_marker: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /next_target_mark + Name: Pure Pursuit Target + Namespaces: + next_target_marker: true + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /mpc_follower/debug/predicted_traj + Name: MPC Predicted Trajectory + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /mpc_follower/debug/filtered_traj + Name: MPC Reference Trajectory + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: Control + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: 0 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 9.120298385620117 + Target Frame: map + Value: TopDownOrtho (rviz) + X: 15.534561157226562 + Y: 22.798494338989258 + Saved: ~ +Window Geometry: + DecisionMakerPanel: + collapsed: false + Displays: + collapsed: false + Height: 1412 + Hide Left Dock: false + Hide Right Dock: false + ImageViewerPlugin: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2711 + X: 4416 + Y: 645 diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv new file mode 100644 index 00000000..3570fabd --- /dev/null +++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv @@ -0,0 +1,539 @@ +DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW +1001,0,1001,1.5706159461937805,0,90000000000,0,0,2,2 +1002,1,1002,1.5706159461937805,0,90000000000,0,0,2,2 +1003,2,1003,1.5710363578994468,0,2378.6207342040943,0,0,2,2 +1004,3,1004,1.5710077368022535,0,-34939.261525991235,0,0,2,2 +1005,4,1005,1.5711231434926902,0,8665.008902131893,0,0,2,2 +1006,5,1006,1.5690903372401281,0,-491.93079701502967,0,0,2,2 +1007,6,1007,1.5689557145684498,0,-7428.169323438051,0,0,2,2 +1008,7,1008,1.5686363058159303,0,-3130.7845890626863,0,0,2,2 +1009,8,1009,1.5682534958301988,0,-2612.2620549962444,0,0,2,2 +1010,9,1010,1.567912565854563,0,-2933.153642870459,0,0,2,2 +1011,10,1011,1.569028273497871,0,896.2921478559839,0,0,2,2 +1012,11,1012,1.5683676551986594,0,-1513.7334239654833,0,0,2,2 +1013,12,1013,1.5684754110339634,0,9280.239879158233,0,0,2,2 +1014,13,1014,1.5684588800570125,0,-60492.49254730128,0,0,2,2 +1015,14,1015,1.5693912035006823,0,1072.5891393053826,0,0,2,2 +1016,15,1016,1.5515668174636283,0,-56.102914171695225,0,0,2,2 +1017,16,1017,1.5593862076728497,0,127.88721028664114,0,0,2,2 +1018,17,1018,1.559631749229936,0,4072.6303598717514,0,0,2,2 +1019,18,1019,1.5603462402901191,0,1399.5976377140162,0,0,2,2 +1020,19,1020,1.5602731051708514,0,-13673.32151792592,0,0,2,2 +1021,20,1021,1.5605402761532297,0,3742.9214471508494,0,0,2,2 +1022,21,1022,1.5596838425998092,0,-1167.6329074288967,0,0,2,2 +1023,22,1023,1.5604369592339504,0,1327.8155795088473,0,0,2,2 +1024,23,1024,1.560366573502377,0,-14207.424965920718,0,0,2,2 +1025,24,1025,1.56027352852355,0,-10747.490220393849,0,0,2,2 +1026,25,1026,1.5603167873838384,0,23116.65155611479,0,0,2,2 +1027,26,1027,1.5602126432082224,0,-9602.073222867975,0,0,2,2 +1028,27,1028,1.5603395866674272,0,7877.522845713829,0,0,2,2 +1029,28,1029,1.5604457063862052,0,9423.319355873213,0,0,2,2 +1030,29,1030,1.560246370088169,0,-5016.647794967425,0,0,2,2 +1031,30,1031,1.559660768578082,0,-1707.6458697164428,0,0,2,2 +1032,31,1032,1.5592028183805353,0,-2183.6435607133826,0,0,2,2 +1033,32,1033,1.5581253798288566,0,-928.1271757372258,0,0,2,2 +1034,33,1034,1.5566612631984011,0,-683.0056972230019,0,0,2,2 +1035,34,1035,1.5555975918432194,0,-940.1400114128027,0,0,2,2 +1036,35,1036,1.5541259437505452,0,-679.5102748938151,0,0,2,2 +1037,36,1037,1.552885481318075,0,-806.1509754943779,0,0,2,2 +1038,37,1038,1.5531588912852945,0,3657.5111367365535,0,0,2,2 +1039,38,1039,1.5533266894392792,0,5959.541128748756,0,0,2,2 +1040,39,1040,1.5541906323996943,0,1157.4838222184007,0,0,2,2 +1041,40,1041,1.5539019306491981,0,-3463.782253766011,0,0,2,2 +1042,41,1042,1.5537746751005848,0,-7858.203519582626,0,0,2,2 +1043,42,1043,1.553458073462205,0,-3158.543351568176,0,0,2,2 +1044,43,1044,1.5542071389727117,0,1334.996720545278,0,0,2,2 +1045,44,1045,1.5542686707312687,0,16251.770198844073,0,0,2,2 +1046,45,1046,1.5547869612148626,0,1929.4199520429686,0,0,2,2 +1047,46,1047,1.5555746441573521,0,1269.5463441666138,0,0,2,2 +1048,47,1048,1.5564501087977414,0,1142.250587705515,0,0,2,2 +1049,48,1049,1.5576003889319332,0,869.3534472822894,0,0,2,2 +1050,49,1050,1.5580756995278244,0,2103.887455159607,0,0,2,2 +1051,50,1051,1.5585502233487594,0,2107.375764676554,0,0,2,2 +1052,51,1052,1.5584516260756238,0,-10142.26832242346,0,0,2,2 +1053,52,1053,1.5584112331616264,0,-24756.817496873653,0,0,2,2 +1054,53,1054,1.5584451473870529,0,29486.151826450732,0,0,2,2 +1055,54,1055,1.5584130588770102,0,-31163.802827570937,0,0,2,2 +1056,55,1056,1.558453395681946,0,24791.254577367206,0,0,2,2 +1057,56,1057,1.5584612741910804,0,126927.56750530448,0,0,2,2 +1058,57,1058,1.558466983527938,0,175151.69010325428,0,0,2,2 +1059,58,1059,1.5584496873239995,0,-57816.15454740148,0,0,2,2 +1060,59,1060,1.5584670021217941,0,57754.0674664271,0,0,2,2 +1061,60,1061,1.5584616988846198,0,-188564.07268518096,0,0,2,2 +1062,61,1062,1.5583642287230526,0,-10259.550039937772,0,0,2,2 +1063,62,1063,1.5584039512178554,0,25174.652422137453,0,0,2,2 +1064,63,1064,1.5584568342185832,0,18909.668253263793,0,0,2,2 +1065,64,1065,1.5584406089211233,0,-61632.15204384786,0,0,2,2 +1066,65,1066,1.5584747613894863,0,29280.46047429793,0,0,2,2 +1067,66,1067,1.5585924861497782,0,8494.389774251948,0,0,2,2 +1068,67,1068,1.5585615744747112,0,-32350.236531457223,0,0,2,2 +1069,68,1069,1.5584424421160337,0,-8394.025024776187,0,0,2,2 +1070,69,1070,1.5584736998776192,0,31992.054109950375,0,0,2,2 +1071,70,1071,1.5584616247474912,0,-82814.84252327424,0,0,2,2 +1072,71,1072,1.55846993294119,0,120363.10614007954,0,0,2,2 +1073,72,1073,1.558246954101369,0,-4484.730482960295,0,0,2,2 +1074,73,1074,1.558474293779886,0,4398.704205631745,0,0,2,2 +1075,74,1075,1.5589662998091534,0,2032.495417768197,0,0,2,2 +1076,75,1076,1.5592385963707622,0,3672.466497892804,0,0,2,2 +1077,76,1077,1.559661432256271,0,2364.9837543870162,0,0,2,2 +1078,77,1078,1.5592922234671693,0,-2708.4945687052523,0,0,2,2 +1079,78,1079,1.5593257540364545,0,29823.531819475797,0,0,2,2 +1080,79,1080,1.5593587691302953,0,30289.17636346063,0,0,2,2 +1081,80,1081,1.5593124961590616,0,-21610.888026868568,0,0,2,2 +1082,81,1082,1.5594323344706573,0,8344.576844289204,0,0,2,2 +1083,82,1083,1.55933102745173,0,-9870.984366025314,0,0,2,2 +1084,83,1084,1.559328555630133,0,-404559.94123097276,0,0,2,2 +1085,84,1085,1.5592803470653718,0,-20743.202062780034,0,0,2,2 +1086,85,1086,1.5593560395197574,0,13211.356509923131,0,0,2,2 +1087,86,1087,1.559392786498505,0,27213.12157025686,0,0,2,2 +1088,87,1088,1.5594015836176165,0,113673.57737659848,0,0,2,2 +1089,88,1089,1.5593621175983554,0,-25338.253482943648,0,0,2,2 +1090,89,1090,1.5593503574191032,0,-85032.7174913827,0,0,2,2 +1091,90,1091,1.559344540912574,0,-171924.50398892158,0,0,2,2 +1092,91,1092,1.5593063957840103,0,-26215.6673120881,0,0,2,2 +1093,92,1093,1.5593010517229873,0,-187123.61174189628,0,0,2,2 +1094,93,1094,1.5593102443018543,0,108783.40174917986,0,0,2,2 +1095,94,1095,1.5592708891791727,0,-25409.65271764711,0,0,2,2 +1096,95,1096,1.55934742287295,0,13066.140553846302,0,0,2,2 +1097,96,1097,1.5593046241271993,0,-23365.170695042812,0,0,2,2 +1098,97,1098,1.5593959499423824,0,10949.806448427382,0,0,2,2 +1099,98,1099,1.5659936909281864,0,151.56702910157335,0,0,2,2 +1100,99,1100,1.5631484014833397,0,-351.4580921850161,0,0,2,2 +1101,100,1101,1.5628303735240179,0,-3144.3776268368265,0,0,2,2 +1102,101,1102,1.5623443678921973,0,-2057.58932515669,0,0,2,2 +1103,102,1103,1.5621970301683075,0,-6787.128059257383,0,0,2,2 +1104,103,1104,1.561518196533237,0,-1473.1149847873494,0,0,2,2 +1105,104,1105,1.56150959557461,0,-116266.1097887333,0,0,2,2 +1106,105,1106,1.5614280308212418,0,-12260.197679807654,0,0,2,2 +1107,106,1107,1.5615331544577284,0,9512.608519086212,0,0,2,2 +1108,107,1108,1.561521769820582,0,-87837.66993589127,0,0,2,2 +1109,108,1109,1.5615063516353334,0,-64858.476134930985,0,0,2,2 +1110,109,1110,1.5613401362473043,0,-6016.290139304405,0,0,2,2 +1111,110,1111,1.5613462370583207,0,163912.63347054776,0,0,2,2 +1112,111,1112,1.5616346082905865,0,3467.752286323709,0,0,2,2 +1113,112,1113,1.5614798702693682,0,-6462.535788724596,0,0,2,2 +1114,113,1114,1.5611376896383071,0,-2922.4330930103124,0,0,2,2 +1115,114,1115,1.5617762833238056,0,1565.9409460327606,0,0,2,2 +1116,115,1116,1.561901748643473,0,7970.329989601685,0,0,2,2 +1117,116,1117,1.5619377171717277,0,27802.082779656434,0,0,2,2 +1118,117,1118,1.5617090872820676,0,-4373.881304350305,0,0,2,2 +1119,118,1119,1.5617712745468253,0,16080.462839074564,0,0,2,2 +1120,119,1120,1.5620671743245305,0,3379.522647010559,0,0,2,2 +1121,120,1121,1.562234529532306,0,5975.314501965154,0,0,2,2 +1122,121,1122,1.5612933601741283,0,-1062.5080293055378,0,0,2,2 +1123,122,1123,1.561698322658157,0,2469.364544715466,0,0,2,2 +1124,123,1124,1.562024360239134,0,3067.1310865553396,0,0,2,2 +1125,124,1125,1.5617193763911459,0,-3278.8621646585816,0,0,2,2 +1126,125,1126,1.5617870168027121,0,14784.061433753894,0,0,2,2 +1127,126,1127,1.5625182243896565,0,1367.6006894005648,0,0,2,2 +1128,127,1128,1.5622360356231595,0,-3543.727173883281,0,0,2,2 +1129,128,1129,1.5625603701776563,0,3083.2360787200023,0,0,2,2 +1130,129,1130,1.562392918909711,0,-5971.886700355639,0,0,2,2 +1131,130,1131,1.562318653263805,0,-13465.176095886947,0,0,2,2 +1132,131,1132,1.562526849807251,0,4803.153709699076,0,0,2,2 +1133,132,1133,1.576137435117669,0,73.47222600592939,0,0,2,2 +1134,133,1134,1.5918120181402755,0,63.79755037551902,0,0,2,2 +1135,134,1135,1.6220769767862286,0,33.04151218900524,0,0,2,2 +1136,135,1136,1.7186597237164492,0,10.353816098464087,0,0,2,2 +1137,136,1137,1.675493482422447,0,-23.166251450736056,0,0,2,2 +1138,137,1138,1.677164571573219,0,598.4121191487482,0,0,2,2 +1139,138,1139,1.6768159724168918,0,-2868.624269019426,0,0,2,2 +1140,139,1140,1.6768476714325655,0,31546.72089172431,0,0,2,2 +1141,140,1141,1.6905273580959745,0,73.10108956478079,0,0,2,2 +1142,141,1142,1.7442418197685423,0,18.616960290801998,0,0,2,2 +1143,142,1143,1.7225513490834752,0,-46.10319501680768,0,0,2,2 +1144,143,1144,1.7485007254879514,0,38.536571531156355,0,0,2,2 +1145,144,1145,1.8148661989890704,0,15.068076022740032,0,0,2,2 +1146,145,1146,1.8169266574025538,0,485.32889256882964,0,0,2,2 +1147,146,1147,1.8036604558276768,0,-75.37952701500924,0,0,2,2 +1148,147,1148,1.8036313187476025,0,-34320.528942918514,0,0,2,2 +1149,148,1149,1.8030076566400433,0,-1603.4323520368343,0,0,2,2 +1150,149,1150,1.8316436098928863,0,34.921135370295964,0,0,2,2 +1151,150,1151,1.876228798227453,0,22.428973328451868,0,0,2,2 +1152,151,1152,1.9324085263911028,0,17.80001492864172,0,0,2,2 +1153,152,1153,1.9158996090297133,0,-60.57332398663326,0,0,2,2 +1154,153,1154,1.9039984847392084,0,-84.02567485139441,0,0,2,2 +1155,154,1155,1.9849339713438288,0,12.355519710224527,0,0,2,2 +1156,155,1156,1.954096844071843,0,-32.42844222096056,0,0,2,2 +1157,156,1157,1.953953083593038,0,-6956.014673251483,0,0,2,2 +1158,157,1158,2.0180868363442888,0,15.592413621554357,0,0,2,2 +1159,158,1159,2.0020751553324976,0,-62.45440433540919,0,0,2,2 +1160,159,1160,2.019090426427932,0,58.770735675691235,0,0,2,2 +1161,160,1161,2.008152822027425,0,-91.42769873389011,0,0,2,2 +1162,161,1162,2.045952035598901,0,26.455576862970116,0,0,2,2 +1163,162,1163,2.083699401528057,0,26.491914743846912,0,0,2,2 +1164,163,1164,2.120776070766995,0,26.971139008080037,0,0,2,2 +1165,164,1165,2.1655019951470065,0,22.35839759293862,0,0,2,2 +1166,165,1166,2.160816734657125,0,-213.43530464520524,0,0,2,2 +1167,166,1167,2.2342858263354426,0,13.611165963211747,0,0,2,2 +1168,167,1168,2.302663449689519,0,14.624667412345591,0,0,2,2 +1169,168,1169,2.2492885285815243,0,-18.735390689883726,0,0,2,2 +1170,169,1170,2.281647456184398,0,30.903372703587333,0,0,2,2 +1171,170,1171,2.3457521763460143,0,15.599475319116399,0,0,2,2 +1172,171,1172,2.474948546053671,0,7.740155565228202,0,0,2,2 +1173,172,1173,2.5173224404688357,0,23.599435780019412,0,0,2,2 +1174,173,1174,2.5368325471829207,0,51.255485920949084,0,0,2,2 +1175,174,1175,2.5456673035159536,0,113.18931301602866,0,0,2,2 +1176,175,1176,2.5547417551394753,0,110.19949650818774,0,0,2,2 +1177,176,1177,2.603195128490749,0,20.638397924335887,0,0,2,2 +1178,177,1178,2.596074157385382,0,-140.43028474673574,0,0,2,2 +1179,178,1179,2.6531475137908123,0,17.521310520031932,0,0,2,2 +1180,179,1180,2.809796039587864,0,6.383717911878494,0,0,2,2 +1181,180,1181,2.7654462594739497,0,-22.54802611042167,0,0,2,2 +1182,181,1182,2.771217664518214,0,173.26803305788565,0,0,2,2 +1183,182,1183,2.8449752803934008,0,13.557921960116037,0,0,2,2 +1184,183,1184,2.8084961628169074,0,-27.4129438000547,0,0,2,2 +1185,184,1185,2.869009574824431,0,16.52526219932323,0,0,2,2 +1186,185,1186,2.884302571393505,0,65.38940851018249,0,0,2,2 +1187,186,1187,2.9905069048410624,0,9.415811648531163,0,0,2,2 +1188,187,1188,2.969341452422632,0,-47.24680485115514,0,0,2,2 +1189,188,1189,3.0383640323914287,0,14.488012480148827,0,0,2,2 +1190,189,1190,3.055186812134341,0,59.44320827366907,0,0,2,2 +1191,190,1191,3.066040854811578,0,92.13157067248235,0,0,2,2 +1192,191,1192,3.0813008039480883,0,65.53101789883758,0,0,2,2 +1193,192,1193,3.0910158947236637,0,102.9326460349796,0,0,2,2 +1194,193,1194,3.10475182392835,0,72.80177300701585,0,0,2,2 +1195,194,1195,3.0928347571304533,0,-83.91326632292699,0,0,2,2 +1196,195,1196,3.093569046175925,0,1361.8615260122626,0,0,2,2 +1197,196,1197,3.093684135325397,0,8688.916414664875,0,0,2,2 +1198,197,1198,3.1063991293121975,0,78.64730420148996,0,0,2,2 +1199,198,1199,3.1272933523549296,0,47.86011894076368,0,0,2,2 +1200,199,1200,3.142608033171954,0,65.29682282952635,0,0,2,2 +1201,200,1201,3.1296736144563315,0,-77.31309941220373,0,0,2,2 +1202,201,1202,3.1295315835431703,0,-7040.7207680544525,0,0,2,2 +1203,202,1203,3.129958005160909,0,2345.0968675165896,0,0,2,2 +1204,203,1204,3.13035122004071,0,2543.1387553419513,0,0,2,2 +1205,204,1205,3.130323608536339,0,-36216.78799352977,0,0,2,2 +1206,205,1206,3.1305842969634368,0,3835.996906852997,0,0,2,2 +1207,206,1207,3.130267219147295,0,-3153.799947810142,0,0,2,2 +1208,207,1208,3.1302595648893865,0,-130646.23794614259,0,0,2,2 +1209,208,1209,3.1301063347354496,0,-6526.130623167103,0,0,2,2 +1210,209,1210,3.130327684273659,0,4517.741523609919,0,0,2,2 +1211,210,1211,3.1304550560171522,0,7851.034872993834,0,0,2,2 +1212,211,1212,3.130259118536645,0,-5103.668769301195,0,0,2,2 +1213,212,1213,3.130053746211815,0,-4869.205238964958,0,0,2,2 +1214,213,1214,3.1300164498391663,0,-26812.258913566046,0,0,2,2 +1215,214,1215,3.129816693563907,0,-5006.100552799646,0,0,2,2 +1216,215,1216,3.129775533359499,0,-24295.31180383525,0,0,2,2 +1217,216,1217,3.1297243109074175,0,-19522.68896470701,0,0,2,2 +1218,217,1218,3.1297590645461977,0,28773.965406220937,0,0,2,2 +1219,218,1219,3.1296879225255916,0,-14056.390182348943,0,0,2,2 +1220,219,1220,3.1297771537246764,0,11206.842564672676,0,0,2,2 +1221,220,1221,3.1295961985796756,0,-5526.231376267536,0,0,2,2 +1222,221,1222,3.129895503753974,0,3341.0715412600025,0,0,2,2 +1223,222,1223,3.1304526609293504,0,1794.8256689405623,0,0,2,2 +1224,223,1224,3.1308640539574997,0,2430.765549184491,0,0,2,2 +1225,224,1225,3.1314005324764267,0,1864.0075319325813,0,0,2,2 +1226,225,1226,3.131838500950461,0,2283.2693659181946,0,0,2,2 +1227,226,1227,3.132310552198498,0,2118.414058130501,0,0,2,2 +1228,227,1228,3.13283405273258,0,1910.2177264318536,0,0,2,2 +1229,228,1229,3.132860414377954,0,37933.89926183089,0,0,2,2 +1230,229,1230,3.1327026639426103,0,-6339.126721396533,0,0,2,2 +1231,230,1231,3.1322941179223944,0,-2447.7046661023764,0,0,2,2 +1232,231,1232,3.132958998066535,0,1504.0304764890313,0,0,2,2 +1233,232,1233,3.1329847186775854,0,38879.32514679646,0,0,2,2 +1234,233,1234,3.132205186132164,0,-1282.820077074694,0,0,2,2 +1235,234,1235,3.1322812556392696,0,13145.871953777567,0,0,2,2 +1236,235,1236,3.1326404704421495,0,2783.8496408909646,0,0,2,2 +1237,236,1237,3.1324687366627106,0,-5822.966240347186,0,0,2,2 +1238,237,1238,3.1327176882712133,0,4016.844904174198,0,0,2,2 +1239,238,1239,3.1325963662922494,0,-8242.529577406198,0,0,2,2 +1240,239,1240,3.1326772233996776,0,12367.496585104844,0,0,2,2 +1241,240,1241,3.1324966080699,0,-5536.628597534322,0,0,2,2 +1242,241,1242,3.1327668588296307,0,3700.2671185690515,0,0,2,2 +1243,242,1243,3.132810249389559,0,23046.487569068846,0,0,2,2 +1244,243,1244,3.1324986288528094,0,-3209.031119806755,0,0,2,2 +1245,244,1245,3.132822786442226,0,3084.9192881761173,0,0,2,2 +1246,245,1246,3.1325701209629964,0,-3957.8022413229055,0,0,2,2 +1247,246,1247,3.132860171494029,0,3447.6751221216164,0,0,2,2 +1248,247,1248,3.1330470784909092,0,5350.254493905768,0,0,2,2 +1249,248,1249,3.13374161105399,0,1439.8173003782692,0,0,2,2 +1250,249,1250,3.134233784801914,0,2031.8028017911413,0,0,2,2 +1251,250,1251,3.133616456325006,0,-1619.8831536311138,0,0,2,2 +1252,251,1252,3.134066252385871,0,2223.2297856879172,0,0,2,2 +1253,252,1253,3.134784454988094,0,1392.3647685274047,0,0,2,2 +1254,253,1254,3.1345994496722183,0,-5405.250088441288,0,0,2,2 +1255,254,1255,3.1362019499703973,0,624.024844885431,0,0,2,2 +1256,255,1256,3.136262484925358,0,16519.38125086305,0,0,2,2 +1257,256,1257,3.1371188806098034,0,1167.6845390079225,0,0,2,2 +1258,257,1258,3.1369988499566253,0,-8331.205184029783,0,0,2,2 +1259,258,1259,3.137867554501641,0,1151.1393669315573,0,0,2,2 +1260,259,1260,3.1708550527699093,0,30.314514664542813,0,0,2,2 +1261,260,1261,3.2356533213820318,0,15.432511105905075,0,0,2,2 +1262,261,1262,3.2018151731438587,0,-29.55244456527002,0,0,2,2 +1263,262,1263,3.2823370968133814,0,12.418978017765573,0,0,2,2 +1264,263,1264,3.36429786440126,0,12.20095942766515,0,0,2,2 +1265,264,1265,3.4485758303486875,0,11.86549756817575,0,0,2,2 +1266,265,1266,3.4777834180506386,0,34.23767858559573,0,0,2,2 +1267,266,1267,3.5907094795636847,0,8.855351781523634,0,0,2,2 +1268,267,1268,3.7473026705560817,0,6.385973704620098,0,0,2,2 +1269,268,1269,3.757926075093773,0,94.13178199625767,0,0,2,2 +1270,269,1270,3.8646434248411556,0,9.370547547958806,0,0,2,2 +1271,270,1271,4.046477554099296,0,5.499517632250184,0,0,2,2 +1272,271,1272,4.26175711340075,0,4.645122849771844,0,0,2,2 +1273,272,1273,4.378686232163503,0,8.552189656273617,0,0,2,2 +1274,273,1274,4.4388107350157435,0,16.63215415614419,0,0,2,2 +1275,274,1275,4.567287183832628,0,7.783527714291725,0,0,2,2 +1276,275,1276,4.619480219254632,0,19.15964442218305,0,0,2,2 +1277,276,1277,4.709966137584271,0,11.051443345659726,0,0,2,2 +1278,277,1278,4.703095070861943,0,-145.53780954425937,0,0,2,2 +1279,278,1279,4.659772006824833,0,-23.082393229237343,0,0,2,2 +1280,279,1280,4.665973477910038,0,161.25206201247974,0,0,2,2 +1281,280,1281,4.636774477273602,0,-34.247747464074145,0,0,2,2 +1282,281,1282,4.652195224894353,0,64.84769899575784,0,0,2,2 +1283,282,1283,4.651902783891486,0,-3419.4931291989383,0,0,2,2 +1284,283,1284,4.687824121303961,0,27.838607135287646,0,0,2,2 +1285,284,1285,4.6736324526187385,0,-70.46387723533208,0,0,2,2 +1286,285,1286,4.684287629155659,0,93.85109636944345,0,0,2,2 +1287,286,1287,4.678651899887421,0,-177.43932548991228,0,0,2,2 +1288,287,1288,4.679214309388232,0,1778.0638459293787,0,0,2,2 +1289,288,1289,4.680302077949644,0,919.3132027110769,0,0,2,2 +1290,289,1290,4.6973518372767735,0,58.65185430558062,0,0,2,2 +1291,290,1291,4.690461092985631,0,-145.12220418415166,0,0,2,2 +1292,291,1292,4.690476707440746,0,64043.22101537215,0,0,2,2 +1293,292,1293,4.690302884709805,0,-5752.987509656575,0,0,2,2 +1294,293,1294,4.689835859312202,0,-2141.2111742384086,0,0,2,2 +1295,294,1295,4.689787908948702,0,-20854.899254200915,0,0,2,2 +1296,295,1296,4.689607694324616,0,-5548.939244371828,0,0,2,2 +1297,296,1297,4.689493209900328,0,-8734.812671833417,0,0,2,2 +1298,297,1298,4.689519331655459,0,38282.26682969883,0,0,2,2 +1299,298,1299,4.689388854307946,0,-7664.165612349602,0,0,2,2 +1300,299,1300,4.68987371253164,0,2062.4585726961486,0,0,2,2 +1301,300,1301,4.689776043791079,0,-10238.690437254516,0,0,2,2 +1302,301,1302,4.689782693126787,0,150390.96293843692,0,0,2,2 +1303,302,1303,4.689816670283746,0,29431.538406950272,0,0,2,2 +1304,303,1304,4.689796642458525,0,-49930.533592485204,0,0,2,2 +1305,304,1305,4.689747163960407,0,-20210.79939837668,0,0,2,2 +1306,305,1306,4.689876308889746,0,7743.238585658182,0,0,2,2 +1307,306,1307,4.689927338270111,0,19596.55384472955,0,0,2,2 +1308,307,1308,4.689980639334232,0,18761.351513307854,0,0,2,2 +1309,308,1309,4.6900762065868395,0,10463.835390423541,0,0,2,2 +1310,309,1310,4.6899051812441055,0,-5847.086659870083,0,0,2,2 +1311,310,1311,4.689963031378174,0,17286.044640952976,0,0,2,2 +1312,311,1312,4.690015400141843,0,19095.35245718582,0,0,2,2 +1313,312,1313,4.6899442234759725,0,-14049.548230019285,0,0,2,2 +1314,313,1314,4.689947326133968,0,322304.2956969203,0,0,2,2 +1315,314,1315,4.689964483759958,0,58283.12148612662,0,0,2,2 +1316,315,1316,4.689997295188629,0,30477.18555715479,0,0,2,2 +1317,316,1317,4.6899609756369145,0,-27533.379482733104,0,0,2,2 +1318,317,1318,4.6898658552838945,0,-10512.997147837103,0,0,2,2 +1319,318,1319,4.6899787594934255,0,8857.06568563471,0,0,2,2 +1320,319,1320,4.689935576462696,0,-23157.24448047331,0,0,2,2 +1321,320,1321,4.6899566101192764,0,47542.850963445235,0,0,2,2 +1322,321,1322,4.689960611514752,0,249912.81320388868,0,0,2,2 +1323,322,1323,4.690077962834115,0,8521.421023858762,0,0,2,2 +1324,323,1324,4.690117658235953,0,25191.834663670186,0,0,2,2 +1325,324,1325,4.690256418250002,0,7206.687076595954,0,0,2,2 +1326,325,1326,4.690478577472271,0,4501.276110846042,0,0,2,2 +1327,326,1327,4.690415949147022,0,-15967.216048522549,0,0,2,2 +1328,327,1328,4.690444340102213,0,35222.48523389742,0,0,2,2 +1329,328,1329,4.684591006921866,0,-170.84282906660815,0,0,2,2 +1330,329,1330,4.687177878574332,0,386.567303811469,0,0,2,2 +1331,330,1331,4.686995455840435,0,-5481.772905385729,0,0,2,2 +1332,331,1332,4.68699624737735,0,1263364.956066549,0,0,2,2 +1333,332,1333,4.68706285354148,0,15013.625436374265,0,0,2,2 +1334,333,1334,4.686911557450675,0,-6609.556100751335,0,0,2,2 +1335,334,1335,4.686865680707907,0,-21797.53704511855,0,0,2,2 +1336,335,1336,4.686820773354471,0,-22268.067999775332,0,0,2,2 +1337,336,1337,4.686725954049043,0,-10546.37550326547,0,0,2,2 +1338,337,1338,4.68673837550253,0,80505.8764747865,0,0,2,2 +1339,338,1339,4.686702762127034,0,-28079.337779137055,0,0,2,2 +1340,339,1340,4.686710912254904,0,122697.46142708715,0,0,2,2 +1341,340,1341,4.689136663573433,0,412.2434119118499,0,0,2,2 +1342,341,1342,4.714259546731511,0,39.80434863736831,0,0,2,2 +1343,342,1343,4.699984756063116,0,-70.05356668480162,0,0,2,2 +1344,343,1344,4.6832033701032545,0,-59.58983378320814,0,0,2,2 +1345,344,1345,4.68680269049533,0,277.83022656209147,0,0,2,2 +1346,345,1346,4.687649092174702,0,1181.4721359512878,0,0,2,2 +1347,346,1347,4.6876939836328555,0,22275.952734298215,0,0,2,2 +1348,347,1348,4.687761437827265,0,14824.87499482366,0,0,2,2 +1349,348,1349,4.687696404440299,0,-15376.717200942252,0,0,2,2 +1350,349,1350,4.687715356450129,0,52764.85232825735,0,0,2,2 +1351,350,1351,4.687582616571676,0,-7533.531080922668,0,0,2,2 +1352,351,1352,4.687707352096387,0,8016.96230738979,0,0,2,2 +1353,352,1353,4.6876684299335345,0,-25692.30296332078,0,0,2,2 +1354,353,1354,4.687624266364536,0,-22643.09752771501,0,0,2,2 +1355,354,1355,4.687720608966777,0,10379.624140726419,0,0,2,2 +1356,355,1356,4.687689292770485,0,-31932.358281027387,0,0,2,2 +1357,356,1357,4.687689884193492,0,1690837.1628970453,0,0,2,2 +1358,357,1358,4.687685465756854,0,-226324.39519933815,0,0,2,2 +1359,358,1359,4.687704008898185,0,53928.295217550374,0,0,2,2 +1360,359,1360,4.6877304231527805,0,37858.34638632972,0,0,2,2 +1361,360,1361,4.687677524239683,0,-18903.98009027763,0,0,2,2 +1362,361,1362,4.687695679221867,0,55081.298888102705,0,0,2,2 +1363,362,1363,4.687701035673197,0,186690.76563969703,0,0,2,2 +1364,363,1364,4.687689154113219,0,-84164.0324843629,0,0,2,2 +1365,364,1365,4.68770174166216,0,79443.5838693156,0,0,2,2 +1366,365,1366,4.6876837324304255,0,-55527.0771541704,0,0,2,2 +1367,366,1367,4.687671490794284,0,-81688.42697691922,0,0,2,2 +1368,367,1368,4.687716206797148,0,22363.35843882257,0,0,2,2 +1369,368,1369,4.6876577767597425,0,-17114.485021718556,0,0,2,2 +1370,369,1370,4.687655188490774,0,-386358.6095590637,0,0,2,2 +1371,370,1371,4.687665058282576,0,101319.25981922718,0,0,2,2 +1372,371,1372,4.687655931651204,0,-109569.45221008976,0,0,2,2 +1373,372,1373,4.687645642127423,0,-97186.2275966764,0,0,2,2 +1374,373,1374,4.687697758904408,0,19187.679243744642,0,0,2,2 +1375,374,1375,4.687658475344309,0,-25455.94130159902,0,0,2,2 +1376,375,1376,4.687625811077754,0,-30614.494231055636,0,0,2,2 +1377,376,1377,4.687660540456129,0,28794.065623782833,0,0,2,2 +1378,377,1378,4.687679585735443,0,52506.44968163131,0,0,2,2 +1379,378,1379,4.687632079348528,0,-21049.801193884927,0,0,2,2 +1380,379,1380,4.687728014737616,0,10423.682120927764,0,0,2,2 +1381,380,1381,4.687692515367519,0,-28169.513916645406,0,0,2,2 +1382,381,1382,4.687670701408633,0,-45842.20614199843,0,0,2,2 +1383,382,1383,4.6876685132754075,0,-457010.5641853212,0,0,2,2 +1384,383,1384,4.687649360193957,0,-52210.919823960416,0,0,2,2 +1385,384,1385,4.687743133627109,0,10664.001160973716,0,0,2,2 +1386,385,1386,4.6876840688895305,0,-16930.5755176804,0,0,2,2 +1387,386,1387,4.687666218497961,0,-56021.18004620228,0,0,2,2 +1388,387,1388,4.687661596380574,0,-216351.06081571136,0,0,2,2 +1389,388,1389,4.687720567316303,0,16957.506060121174,0,0,2,2 +1390,389,1390,4.687640571279501,0,-12500.61928038758,0,0,2,2 +1391,390,1391,4.687642280815317,0,584954.1089631345,0,0,2,2 +1392,391,1392,4.6876491204650765,0,146206.3168685606,0,0,2,2 +1393,392,1393,4.687699726107593,0,19760.64229743995,0,0,2,2 +1394,393,1394,4.687957029171137,0,3886.467522868451,0,0,2,2 +1395,394,1395,4.688037445066655,0,12435.352408410188,0,0,2,2 +1396,395,1396,4.688290751141772,0,3947.793196581492,0,0,2,2 +1397,396,1397,4.6895811702077905,0,774.9420528058007,0,0,2,2 +1398,397,1398,4.688626779234222,0,-1047.7886188101368,0,0,2,2 +1399,398,1399,4.689114982797937,0,2048.325891745624,0,0,2,2 +1400,399,1400,4.689167404357547,0,19076.120730615625,0,0,2,2 +1401,400,1401,4.68819941339195,0,-1033.067492921702,0,0,2,2 +1402,401,1402,4.68774385311159,0,-2195.0991846980946,0,0,2,2 +1403,402,1403,4.68775815043782,0,69943.1476822489,0,0,2,2 +1404,403,1404,4.687759731188219,0,632610.9426376662,0,0,2,2 +1405,404,1405,4.699177624229751,0,87.58183286203027,0,0,2,2 +1406,405,1406,4.695531633832969,0,-274.2738984947965,0,0,2,2 +1407,406,1407,4.695247715644894,0,-3522.1413843853197,0,0,2,2 +1408,407,1408,4.754735647601379,0,16.810132191710583,0,0,2,2 +1409,408,1409,4.724397302964348,0,-32.961587455216275,0,0,2,2 +1410,409,1410,4.759533786047175,0,28.460446586036806,0,0,2,2 +1411,410,1411,4.743882767713707,0,-63.89360607044966,0,0,2,2 +1412,411,1412,4.744282268105393,0,2503.126456971458,0,0,2,2 +1413,412,1413,4.745442432748384,0,861.9466263181986,0,0,2,2 +1414,413,1414,4.813486442426927,0,14.696370844755526,0,0,2,2 +1415,414,1415,4.776165988226798,0,-26.79495792407948,0,0,2,2 +1416,415,1416,4.819133963999859,0,23.273146616949663,0,0,2,2 +1417,416,1417,4.815819733304253,0,-301.72914677476547,0,0,2,2 +1418,417,1418,4.82338051411354,0,132.26147209183162,0,0,2,2 +1419,418,1419,4.883073471345198,0,16.752395029101688,0,0,2,2 +1420,419,1420,4.884712249460814,0,610.2107359567673,0,0,2,2 +1421,420,1421,4.961541038870127,0,13.015954145423295,0,0,2,2 +1422,421,1422,4.999906861541805,0,26.064865298410677,0,0,2,2 +1423,422,1423,4.993773766242908,0,-163.04980621772725,0,0,2,2 +1424,423,1424,5.027284605822452,0,29.841090600739474,0,0,2,2 +1425,424,1425,5.049561561017202,0,44.8894380429352,0,0,2,2 +1426,425,1426,5.068208436003781,0,53.62828896100618,0,0,2,2 +1427,426,1427,5.050902273466685,0,-57.78288501893302,0,0,2,2 +1428,427,1428,5.071232352781532,0,49.18819963824127,0,0,2,2 +1429,428,1429,5.099616439035059,0,35.23100906148571,0,0,2,2 +1430,429,1430,5.0834811075303215,0,-61.975795149072646,0,0,2,2 +1431,430,1431,5.198289675352311,0,8.710151332525102,0,0,2,2 +1432,431,1432,5.157266264870209,0,-24.37632532859001,0,0,2,2 +1433,432,1433,5.2054644475826946,0,20.747670217469807,0,0,2,2 +1434,433,1434,5.211499235975039,0,165.7058930630452,0,0,2,2 +1435,434,1435,5.201878966277255,0,-103.9471897789248,0,0,2,2 +1436,435,1436,5.278536273467066,0,13.045070804847205,0,0,2,2 +1437,436,1437,5.299850103443958,0,46.91789326855794,0,0,2,2 +1438,437,1438,5.292430313476906,0,-134.7747044647677,0,0,2,2 +1439,438,1439,5.335799600526837,0,23.057792000332128,0,0,2,2 +1440,439,1440,5.38607544957814,0,19.890265781082434,0,0,2,2 +1441,440,1441,5.354411048658623,0,-31.581207000939052,0,0,2,2 +1442,441,1442,5.354553509767046,0,7019.459634075851,0,0,2,2 +1443,442,1443,5.418189239753329,0,15.71444218861867,0,0,2,2 +1444,443,1444,5.478070005053282,0,16.69985336678365,0,0,2,2 +1445,444,1445,5.457578047651754,0,-48.79963296846651,0,0,2,2 +1446,445,1446,5.44743639717364,0,-98.60327982689128,0,0,2,2 +1447,446,1447,5.447582422152287,0,6848.143442770463,0,0,2,2 +1448,447,1448,5.552216302498086,0,9.557133852774605,0,0,2,2 +1449,448,1449,5.502154534883066,0,-19.975323438239375,0,0,2,2 +1450,449,1450,5.594327596757653,0,10.849156789004411,0,0,2,2 +1451,450,1451,5.703767870137007,0,9.13740407549686,0,0,2,2 +1452,451,1452,5.790683385369762,0,11.505425669077031,0,0,2,2 +1453,452,1453,5.851272330781886,0,16.50466092779857,0,0,2,2 +1454,453,1454,5.784744718309785,0,-15.031352589413242,0,0,2,2 +1455,454,1455,5.866744337751296,0,12.195178548520975,0,0,2,2 +1456,455,1456,5.83407175340783,0,-30.606700390995794,0,0,2,2 +1457,456,1457,5.83298837237763,0,-923.0362837492128,0,0,2,2 +1458,457,1458,5.909277584800559,0,13.108013154680902,0,0,2,2 +1459,458,1459,5.885308774937403,0,-41.720886673524866,0,0,2,2 +1460,459,1460,5.919908712922938,0,28.901785905456688,0,0,2,2 +1461,460,1461,5.965351169098282,0,22.00585276775979,0,0,2,2 +1462,461,1462,6.016863401140053,0,19.412864874290744,0,0,2,2 +1463,462,1463,6.060070083297614,0,23.144568156224413,0,0,2,2 +1464,463,1464,6.124157266259176,0,15.603744052843995,0,0,2,2 +1465,464,1465,6.119357144906505,0,-208.3280664235001,0,0,2,2 +1466,465,1466,6.130669368179921,0,88.39995249652004,0,0,2,2 +1467,466,1467,6.180864201080793,0,19.922369339786027,0,0,2,2 +1468,467,1468,6.230570579446633,0,20.11814243717314,0,0,2,2 +1469,468,1469,6.263350807100293,0,30.506194483013054,0,0,2,2 +1470,469,1470,6.193143317898733,0,-14.243494695118356,0,0,2,2 +1471,470,1471,6.195834121923333,0,371.6361321217441,0,0,2,2 +1472,471,1472,6.208803875641413,0,77.1024663796037,0,0,2,2 +1473,472,1473,6.221901699465272,0,76.34856090966778,0,0,2,2 +1474,473,1474,6.256501196726211,0,28.9021540532308,0,0,2,2 +1475,474,1475,6.23629215057456,0,-49.48279065205987,0,0,2,2 +1476,475,1476,6.235907617862001,0,-2600.5589832538453,0,0,2,2 +1477,476,1477,6.237021459432858,0,897.7937492762666,0,0,2,2 +1478,477,1478,6.237655749087532,0,1576.5667824346535,0,0,2,2 +1479,478,1479,6.244179133136743,0,153.29466921710022,0,0,2,2 +1480,479,1480,6.270392764015719,0,38.1480919074826,0,0,2,2 +1481,480,1481,6.256087261027579,0,-69.903169488625,0,0,2,2 +1482,481,1482,6.257661319758907,0,635.3003100185017,0,0,2,2 +1483,482,1483,6.267836945548009,0,98.27405416883752,0,0,2,2 +1484,483,1484,6.269723566488711,0,530.0481821365647,0,0,2,2 +1485,484,1485,6.267054253560375,0,-374.6282383697463,0,0,2,2 +1486,485,1486,0.013744254559647902,0,-0.15991530887798605,0,0,2,2 +1487,486,1487,0.0010119061751368357,0,-78.54010664807936,0,0,2,2 +1488,487,1488,0.0012243999420399345,0,4706.0203909699585,0,0,2,2 +1489,488,1489,0.0018815871511780942,0,1521.6364318949657,0,0,2,2 +1490,489,1490,0.002008029429596202,0,7908.747078198724,0,0,2,2 +1491,490,1491,0.0018352499454211354,0,-5787.724189445794,0,0,2,2 +1492,491,1492,0.0011422250006752636,0,-1442.9495035949885,0,0,2,2 +1493,492,1493,0.001342209822478479,0,5000.379483719008,0,0,2,2 +1494,493,1494,6.27518733217875,0,0.1593918849600848,0,0,2,2 +1495,494,1495,6.277509724011835,0,430.5905600226852,0,0,2,2 +1496,495,1496,6.278068934614515,0,1788.2350499208055,0,0,2,2 +1497,496,1497,6.27766295708146,0,-2463.1904935097405,0,0,2,2 +1498,497,1498,6.277751071486938,0,11348.882110444736,0,0,2,2 +1499,498,1499,6.277640581367299,0,-9050.58301386519,0,0,2,2 +1500,499,1500,6.2774103081305626,0,-4342.667060107362,0,0,2,2 +1501,500,1501,6.267469573203085,0,-100.59618401410951,0,0,2,2 +1502,501,1502,6.268776934010963,0,764.899784339935,0,0,2,2 +1503,502,1503,6.269099769240327,0,3097.5553751321304,0,0,2,2 +1504,503,1504,6.269088227498937,0,-86642.03833671357,0,0,2,2 +1505,504,1505,6.269037911255563,0,-19874.297700944935,0,0,2,2 +1506,505,1506,6.26957021841891,0,1878.6145835655457,0,0,2,2 +1507,506,1507,6.268937417154099,0,-1580.2749703702154,0,0,2,2 +1508,507,1508,6.25233126629231,0,-60.21865080733453,0,0,2,2 +1509,508,1509,6.258024899264237,0,175.634784491826,0,0,2,2 +1510,509,1510,6.230735642842278,0,-36.644457604030876,0,0,2,2 +1511,510,1511,6.240254079440697,0,105.05926993999557,0,0,2,2 +1512,511,1512,6.239790331359758,0,-2156.343155047787,0,0,2,2 +1513,512,1513,6.239202869296887,0,-1702.2375795858684,0,0,2,2 +1514,513,1514,6.238970234292401,0,-4298.579236645538,0,0,2,2 +1515,514,1515,6.238925065169713,0,-22139.017552296125,0,0,2,2 +1516,515,1516,6.238863666976653,0,-16287.124264657097,0,0,2,2 +1517,516,1517,6.238732914238007,0,-7648.023363449819,0,0,2,2 +1518,517,1518,6.264384233905774,0,38.98434906865983,0,0,2,2 +1519,518,1519,6.254531263325934,0,-101.49223443801617,0,0,2,2 +1520,519,1520,0.010624767483427618,0,-0.1601561459425839,0,0,2,2 +1521,520,1521,6.272689144792406,0,0.1596917469618436,0,0,2,2 +1522,521,1522,6.272761513075674,0,13818.208127091919,0,0,2,2 +1523,522,1523,6.272970784747892,0,4778.477609518447,0,0,2,2 +1524,523,1524,6.27296208880721,0,-114996.18460447,0,0,2,2 +1525,524,1525,6.273040252520847,0,12793.660299308754,0,0,2,2 +1526,525,1526,6.273027181376255,0,-76504.3943162592,0,0,2,2 +1527,526,1527,6.272997881943862,0,-34130.35401441668,0,0,2,2 +1528,527,1528,6.264496571896351,0,-117.62892947220149,0,0,2,2 +1529,528,1529,6.256412251422299,0,-123.69623436004015,0,0,2,2 +1530,529,1530,6.262334255024012,0,168.86176828916354,0,0,2,2 +1531,530,1531,6.262342165248228,0,126418.66686483091,0,0,2,2 +1532,531,1532,6.262333893420006,0,-120892.25902943166,0,0,2,2 +1533,532,1533,6.262315140960322,0,-53326.33781473414,0,0,2,2 +1534,533,1534,6.2621862311902685,0,-7757.363926623419,0,0,2,2 +1535,534,1535,6.262360065436234,0,5752.60642368762,0,0,2,2 +1536,535,1536,6.26219896741017,0,-6207.400701499976,0,0,2,2 +1537,536,1537,6.262701530291994,0,1989.8007516411894,0,0,2,2 +1538,537,1538,6.263282807370463,0,1720.3499622483916,0,0,2,2 diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv new file mode 100644 index 00000000..045416d0 --- /dev/null +++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv @@ -0,0 +1,538 @@ +LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,CrossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG +1001,1001,0,1002,1001,1002,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1002,1002,1001,1003,1002,1003,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1003,1003,1002,1004,1003,1004,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1004,1004,1003,1005,1004,1005,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1005,1005,1004,1006,1005,1006,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1006,1006,1005,1007,1006,1007,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1007,1007,1006,1008,1007,1008,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1008,1008,1007,1009,1008,1009,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1009,1009,1008,1010,1009,1010,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1010,1010,1009,1011,1010,1011,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1011,1011,1010,1012,1011,1012,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1012,1012,1011,1013,1012,1013,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1013,1013,1012,1014,1013,1014,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1014,1014,1013,1015,1014,1015,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1015,1015,1014,1016,1015,1016,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1016,1016,1015,1017,1016,1017,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1017,1017,1016,1018,1017,1018,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1018,1018,1017,1019,1018,1019,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1019,1019,1018,1020,1019,1020,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1020,1020,1019,1021,1020,1021,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1021,1021,1020,1022,1021,1022,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1022,1022,1021,1023,1022,1023,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1023,1023,1022,1024,1023,1024,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1024,1024,1023,1025,1024,1025,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1025,1025,1024,1026,1025,1026,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1026,1026,1025,1027,1026,1027,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1027,1027,1026,1028,1027,1028,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1028,1028,1027,1029,1028,1029,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1029,1029,1028,1030,1029,1030,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1030,1030,1029,1031,1030,1031,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1031,1031,1030,1032,1031,1032,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1032,1032,1031,1033,1032,1033,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1033,1033,1032,1034,1033,1034,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1034,1034,1033,1035,1034,1035,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1035,1035,1034,1036,1035,1036,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1036,1036,1035,1037,1036,1037,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1037,1037,1036,1038,1037,1038,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1038,1038,1037,1039,1038,1039,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1039,1039,1038,1040,1039,1040,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1040,1040,1039,1041,1040,1041,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1041,1041,1040,1042,1041,1042,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1042,1042,1041,1043,1042,1043,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1043,1043,1042,1044,1043,1044,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1044,1044,1043,1045,1044,1045,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1045,1045,1044,1046,1045,1046,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1046,1046,1045,1047,1046,1047,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1047,1047,1046,1048,1047,1048,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1048,1048,1047,1049,1048,1049,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1049,1049,1048,1050,1049,1050,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1050,1050,1049,1051,1050,1051,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1051,1051,1050,1052,1051,1052,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1052,1052,1051,1053,1052,1053,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1053,1053,1052,1054,1053,1054,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1054,1054,1053,1055,1054,1055,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1055,1055,1054,1056,1055,1056,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1056,1056,1055,1057,1056,1057,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1057,1057,1056,1058,1057,1058,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1058,1058,1057,1059,1058,1059,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1059,1059,1058,1060,1059,1060,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1060,1060,1059,1061,1060,1061,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1061,1061,1060,1062,1061,1062,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1062,1062,1061,1063,1062,1063,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1063,1063,1062,1064,1063,1064,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1064,1064,1063,1065,1064,1065,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1065,1065,1064,1066,1065,1066,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1066,1066,1065,1067,1066,1067,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1067,1067,1066,1068,1067,1068,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1068,1068,1067,1069,1068,1069,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1069,1069,1068,1070,1069,1070,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1070,1070,1069,1071,1070,1071,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1071,1071,1070,1072,1071,1072,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1072,1072,1071,1073,1072,1073,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1073,1073,1072,1074,1073,1074,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1074,1074,1073,1075,1074,1075,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1075,1075,1074,1076,1075,1076,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1076,1076,1075,1077,1076,1077,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1077,1077,1076,1078,1077,1078,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1078,1078,1077,1079,1078,1079,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1079,1079,1078,1080,1079,1080,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1080,1080,1079,1081,1080,1081,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1081,1081,1080,1082,1081,1082,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1082,1082,1081,1083,1082,1083,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1083,1083,1082,1084,1083,1084,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1084,1084,1083,1085,1084,1085,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1085,1085,1084,1086,1085,1086,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1086,1086,1085,1087,1086,1087,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1087,1087,1086,1088,1087,1088,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1088,1088,1087,1089,1088,1089,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1089,1089,1088,1090,1089,1090,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1090,1090,1089,1091,1090,1091,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1091,1091,1090,1092,1091,1092,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1092,1092,1091,1093,1092,1093,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1093,1093,1092,1094,1093,1094,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1094,1094,1093,1095,1094,1095,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1095,1095,1094,1096,1095,1096,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1096,1096,1095,1097,1096,1097,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1097,1097,1096,1098,1097,1098,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1098,1098,1097,1099,1098,1099,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1099,1099,1098,1100,1099,1100,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1100,1100,1099,1101,1100,1101,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1101,1101,1100,1102,1101,1102,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1102,1102,1101,1103,1102,1103,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1103,1103,1102,1104,1103,1104,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1104,1104,1103,1105,1104,1105,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1105,1105,1104,1106,1105,1106,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1106,1106,1105,1107,1106,1107,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1107,1107,1106,1108,1107,1108,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1108,1108,1107,1109,1108,1109,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1109,1109,1108,1110,1109,1110,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1110,1110,1109,1111,1110,1111,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1111,1111,1110,1112,1111,1112,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1112,1112,1111,1113,1112,1113,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1113,1113,1112,1114,1113,1114,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1114,1114,1113,1115,1114,1115,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1115,1115,1114,1116,1115,1116,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1116,1116,1115,1117,1116,1117,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1117,1117,1116,1118,1117,1118,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1118,1118,1117,1119,1118,1119,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1119,1119,1118,1120,1119,1120,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1120,1120,1119,1121,1120,1121,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1121,1121,1120,1122,1121,1122,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1122,1122,1121,1123,1122,1123,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1123,1123,1122,1124,1123,1124,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1124,1124,1123,1125,1124,1125,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1125,1125,1124,1126,1125,1126,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1126,1126,1125,1127,1126,1127,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1127,1127,1126,1128,1127,1128,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1128,1128,1127,1129,1128,1129,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1129,1129,1128,1130,1129,1130,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1130,1130,1129,1131,1130,1131,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1131,1131,1130,1132,1131,1132,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1132,1132,1131,1133,1132,1133,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1133,1133,1132,1134,1133,1134,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1134,1134,1133,1135,1134,1135,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1135,1135,1134,1136,1135,1136,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1136,1136,1135,1137,1136,1137,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1137,1137,1136,1138,1137,1138,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1138,1138,1137,1139,1138,1139,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1139,1139,1138,1140,1139,1140,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1140,1140,1139,1141,1140,1141,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1141,1141,1140,1142,1141,1142,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1142,1142,1141,1143,1142,1143,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1143,1143,1142,1144,1143,1144,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1144,1144,1143,1145,1144,1145,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1145,1145,1144,1146,1145,1146,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1146,1146,1145,1147,1146,1147,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1147,1147,1146,1148,1147,1148,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1148,1148,1147,1149,1148,1149,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1149,1149,1148,1150,1149,1150,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1150,1150,1149,1151,1150,1151,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1151,1151,1150,1152,1151,1152,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1152,1152,1151,1153,1152,1153,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1153,1153,1152,1154,1153,1154,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1154,1154,1153,1155,1154,1155,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1155,1155,1154,1156,1155,1156,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1156,1156,1155,1157,1156,1157,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1157,1157,1156,1158,1157,1158,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1158,1158,1157,1159,1158,1159,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1159,1159,1158,1160,1159,1160,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1160,1160,1159,1161,1160,1161,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1161,1161,1160,1162,1161,1162,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1162,1162,1161,1163,1162,1163,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1163,1163,1162,1164,1163,1164,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1164,1164,1163,1165,1164,1165,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1165,1165,1164,1166,1165,1166,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1166,1166,1165,1167,1166,1167,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1167,1167,1166,1168,1167,1168,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1168,1168,1167,1169,1168,1169,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1169,1169,1168,1170,1169,1170,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1170,1170,1169,1171,1170,1171,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1171,1171,1170,1172,1171,1172,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1172,1172,1171,1173,1172,1173,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1173,1173,1172,1174,1173,1174,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1174,1174,1173,1175,1174,1175,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1175,1175,1174,1176,1175,1176,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1176,1176,1175,1177,1176,1177,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1177,1177,1176,1178,1177,1178,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1178,1178,1177,1179,1178,1179,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1179,1179,1178,1180,1179,1180,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1180,1180,1179,1181,1180,1181,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1181,1181,1180,1182,1181,1182,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1182,1182,1181,1183,1182,1183,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1183,1183,1182,1184,1183,1184,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1184,1184,1183,1185,1184,1185,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1185,1185,1184,1186,1185,1186,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1186,1186,1185,1187,1186,1187,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1187,1187,1186,1188,1187,1188,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1188,1188,1187,1189,1188,1189,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1189,1189,1188,1190,1189,1190,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1190,1190,1189,1191,1190,1191,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1191,1191,1190,1192,1191,1192,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1192,1192,1191,1193,1192,1193,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1193,1193,1192,1194,1193,1194,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1194,1194,1193,1195,1194,1195,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1195,1195,1194,1196,1195,1196,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1196,1196,1195,1197,1196,1197,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1197,1197,1196,1198,1197,1198,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1198,1198,1197,1199,1198,1199,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1199,1199,1198,1200,1199,1200,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1200,1200,1199,1201,1200,1201,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1201,1201,1200,1202,1201,1202,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1202,1202,1201,1203,1202,1203,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1203,1203,1202,1204,1203,1204,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1204,1204,1203,1205,1204,1205,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1205,1205,1204,1206,1205,1206,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1206,1206,1205,1207,1206,1207,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1207,1207,1206,1208,1207,1208,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1208,1208,1207,1209,1208,1209,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1209,1209,1208,1210,1209,1210,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1210,1210,1209,1211,1210,1211,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1211,1211,1210,1212,1211,1212,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1212,1212,1211,1213,1212,1213,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1213,1213,1212,1214,1213,1214,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1214,1214,1213,1215,1214,1215,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1215,1215,1214,1216,1215,1216,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1216,1216,1215,1217,1216,1217,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1217,1217,1216,1218,1217,1218,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1218,1218,1217,1219,1218,1219,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1219,1219,1218,1220,1219,1220,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1220,1220,1219,1221,1220,1221,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1221,1221,1220,1222,1221,1222,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1222,1222,1221,1223,1222,1223,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1223,1223,1222,1224,1223,1224,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1224,1224,1223,1225,1224,1225,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1225,1225,1224,1226,1225,1226,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1226,1226,1225,1227,1226,1227,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1227,1227,1226,1228,1227,1228,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1228,1228,1227,1229,1228,1229,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1229,1229,1228,1230,1229,1230,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1230,1230,1229,1231,1230,1231,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1231,1231,1230,1232,1231,1232,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1232,1232,1231,1233,1232,1233,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1233,1233,1232,1234,1233,1234,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1234,1234,1233,1235,1234,1235,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1235,1235,1234,1236,1235,1236,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1236,1236,1235,1237,1236,1237,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1237,1237,1236,1238,1237,1238,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1238,1238,1237,1239,1238,1239,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1239,1239,1238,1240,1239,1240,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1240,1240,1239,1241,1240,1241,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1241,1241,1240,1242,1241,1242,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1242,1242,1241,1243,1242,1243,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1243,1243,1242,1244,1243,1244,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1244,1244,1243,1245,1244,1245,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1245,1245,1244,1246,1245,1246,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1246,1246,1245,1247,1246,1247,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1247,1247,1246,1248,1247,1248,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1248,1248,1247,1249,1248,1249,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1249,1249,1248,1250,1249,1250,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1250,1250,1249,1251,1250,1251,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1251,1251,1250,1252,1251,1252,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1252,1252,1251,1253,1252,1253,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1253,1253,1252,1254,1253,1254,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1254,1254,1253,1255,1254,1255,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1255,1255,1254,1256,1255,1256,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1256,1256,1255,1257,1256,1257,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1257,1257,1256,1258,1257,1258,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1258,1258,1257,1259,1258,1259,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1259,1259,1258,1260,1259,1260,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1260,1260,1259,1261,1260,1261,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1261,1261,1260,1262,1261,1262,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1262,1262,1261,1263,1262,1263,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1263,1263,1262,1264,1263,1264,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1264,1264,1263,1265,1264,1265,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1265,1265,1264,1266,1265,1266,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1266,1266,1265,1267,1266,1267,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1267,1267,1266,1268,1267,1268,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1268,1268,1267,1269,1268,1269,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1269,1269,1268,1270,1269,1270,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1270,1270,1269,1271,1270,1271,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1271,1271,1270,1272,1271,1272,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1272,1272,1271,1273,1272,1273,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1273,1273,1272,1274,1273,1274,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1274,1274,1273,1275,1274,1275,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1275,1275,1274,1276,1275,1276,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1276,1276,1275,1277,1276,1277,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1277,1277,1276,1278,1277,1278,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1278,1278,1277,1279,1278,1279,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1279,1279,1278,1280,1279,1280,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1280,1280,1279,1281,1280,1281,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1281,1281,1280,1282,1281,1282,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1282,1282,1281,1283,1282,1283,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1283,1283,1282,1284,1283,1284,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1284,1284,1283,1285,1284,1285,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1285,1285,1284,1286,1285,1286,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1286,1286,1285,1287,1286,1287,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1287,1287,1286,1288,1287,1288,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1288,1288,1287,1289,1288,1289,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1289,1289,1288,1290,1289,1290,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1290,1290,1289,1291,1290,1291,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1291,1291,1290,1292,1291,1292,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1292,1292,1291,1293,1292,1293,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1293,1293,1292,1294,1293,1294,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1294,1294,1293,1295,1294,1295,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1295,1295,1294,1296,1295,1296,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1296,1296,1295,1297,1296,1297,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1297,1297,1296,1298,1297,1298,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1298,1298,1297,1299,1298,1299,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1299,1299,1298,1300,1299,1300,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1300,1300,1299,1301,1300,1301,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1301,1301,1300,1302,1301,1302,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1302,1302,1301,1303,1302,1303,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1303,1303,1302,1304,1303,1304,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1304,1304,1303,1305,1304,1305,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1305,1305,1304,1306,1305,1306,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1306,1306,1305,1307,1306,1307,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1307,1307,1306,1308,1307,1308,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1308,1308,1307,1309,1308,1309,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1309,1309,1308,1310,1309,1310,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1310,1310,1309,1311,1310,1311,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1311,1311,1310,1312,1311,1312,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1312,1312,1311,1313,1312,1313,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1313,1313,1312,1314,1313,1314,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1314,1314,1313,1315,1314,1315,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1315,1315,1314,1316,1315,1316,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1316,1316,1315,1317,1316,1317,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1317,1317,1316,1318,1317,1318,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1318,1318,1317,1319,1318,1319,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1319,1319,1318,1320,1319,1320,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1320,1320,1319,1321,1320,1321,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1321,1321,1320,1322,1321,1322,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1322,1322,1321,1323,1322,1323,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1323,1323,1322,1324,1323,1324,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1324,1324,1323,1325,1324,1325,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1325,1325,1324,1326,1325,1326,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1326,1326,1325,1327,1326,1327,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1327,1327,1326,1328,1327,1328,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1328,1328,1327,1329,1328,1329,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1329,1329,1328,1330,1329,1330,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1330,1330,1329,1331,1330,1331,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1331,1331,1330,1332,1331,1332,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1332,1332,1331,1333,1332,1333,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1333,1333,1332,1334,1333,1334,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1334,1334,1333,1335,1334,1335,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1335,1335,1334,1336,1335,1336,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1336,1336,1335,1337,1336,1337,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1337,1337,1336,1338,1337,1338,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1338,1338,1337,1339,1338,1339,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1339,1339,1338,1340,1339,1340,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1340,1340,1339,1341,1340,1341,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1341,1341,1340,1342,1341,1342,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1342,1342,1341,1343,1342,1343,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1343,1343,1342,1344,1343,1344,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1344,1344,1343,1345,1344,1345,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1345,1345,1344,1346,1345,1346,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1346,1346,1345,1347,1346,1347,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1347,1347,1346,1348,1347,1348,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1348,1348,1347,1349,1348,1349,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1349,1349,1348,1350,1349,1350,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1350,1350,1349,1351,1350,1351,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1351,1351,1350,1352,1351,1352,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1352,1352,1351,1353,1352,1353,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1353,1353,1352,1354,1353,1354,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1354,1354,1353,1355,1354,1355,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1355,1355,1354,1356,1355,1356,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1356,1356,1355,1357,1356,1357,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1357,1357,1356,1358,1357,1358,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1358,1358,1357,1359,1358,1359,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1359,1359,1358,1360,1359,1360,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1360,1360,1359,1361,1360,1361,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1361,1361,1360,1362,1361,1362,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1362,1362,1361,1363,1362,1363,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1363,1363,1362,1364,1363,1364,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1364,1364,1363,1365,1364,1365,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1365,1365,1364,1366,1365,1366,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1366,1366,1365,1367,1366,1367,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1367,1367,1366,1368,1367,1368,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1368,1368,1367,1369,1368,1369,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1369,1369,1368,1370,1369,1370,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1370,1370,1369,1371,1370,1371,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1371,1371,1370,1372,1371,1372,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1372,1372,1371,1373,1372,1373,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1373,1373,1372,1374,1373,1374,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1374,1374,1373,1375,1374,1375,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1375,1375,1374,1376,1375,1376,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1376,1376,1375,1377,1376,1377,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1377,1377,1376,1378,1377,1378,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1378,1378,1377,1379,1378,1379,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1379,1379,1378,1380,1379,1380,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1380,1380,1379,1381,1380,1381,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1381,1381,1380,1382,1381,1382,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1382,1382,1381,1383,1382,1383,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1383,1383,1382,1384,1383,1384,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1384,1384,1383,1385,1384,1385,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1385,1385,1384,1386,1385,1386,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1386,1386,1385,1387,1386,1387,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1387,1387,1386,1388,1387,1388,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1388,1388,1387,1389,1388,1389,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1389,1389,1388,1390,1389,1390,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1390,1390,1389,1391,1390,1391,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1391,1391,1390,1392,1391,1392,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1392,1392,1391,1393,1392,1393,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1393,1393,1392,1394,1393,1394,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1394,1394,1393,1395,1394,1395,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1395,1395,1394,1396,1395,1396,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1396,1396,1395,1397,1396,1397,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1397,1397,1396,1398,1397,1398,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1398,1398,1397,1399,1398,1399,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1399,1399,1398,1400,1399,1400,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1400,1400,1399,1401,1400,1401,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1401,1401,1400,1402,1401,1402,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1402,1402,1401,1403,1402,1403,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1403,1403,1402,1404,1403,1404,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1404,1404,1403,1405,1404,1405,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1405,1405,1404,1406,1405,1406,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1406,1406,1405,1407,1406,1407,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1407,1407,1406,1408,1407,1408,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1408,1408,1407,1409,1408,1409,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1409,1409,1408,1410,1409,1410,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1410,1410,1409,1411,1410,1411,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1411,1411,1410,1412,1411,1412,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1412,1412,1411,1413,1412,1413,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1413,1413,1412,1414,1413,1414,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1414,1414,1413,1415,1414,1415,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1415,1415,1414,1416,1415,1416,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1416,1416,1415,1417,1416,1417,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1417,1417,1416,1418,1417,1418,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1418,1418,1417,1419,1418,1419,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1419,1419,1418,1420,1419,1420,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1420,1420,1419,1421,1420,1421,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1421,1421,1420,1422,1421,1422,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1422,1422,1421,1423,1422,1423,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1423,1423,1422,1424,1423,1424,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1424,1424,1423,1425,1424,1425,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1425,1425,1424,1426,1425,1426,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1426,1426,1425,1427,1426,1427,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1427,1427,1426,1428,1427,1428,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1428,1428,1427,1429,1428,1429,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1429,1429,1428,1430,1429,1430,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1430,1430,1429,1431,1430,1431,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1431,1431,1430,1432,1431,1432,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1432,1432,1431,1433,1432,1433,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1433,1433,1432,1434,1433,1434,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1434,1434,1433,1435,1434,1435,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1435,1435,1434,1436,1435,1436,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1436,1436,1435,1437,1436,1437,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1437,1437,1436,1438,1437,1438,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1438,1438,1437,1439,1438,1439,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1439,1439,1438,1440,1439,1440,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1440,1440,1439,1441,1440,1441,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1441,1441,1440,1442,1441,1442,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1442,1442,1441,1443,1442,1443,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1443,1443,1442,1444,1443,1444,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1444,1444,1443,1445,1444,1445,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1445,1445,1444,1446,1445,1446,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1446,1446,1445,1447,1446,1447,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1447,1447,1446,1448,1447,1448,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1448,1448,1447,1449,1448,1449,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1449,1449,1448,1450,1449,1450,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1450,1450,1449,1451,1450,1451,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1451,1451,1450,1452,1451,1452,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1452,1452,1451,1453,1452,1453,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1453,1453,1452,1454,1453,1454,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1454,1454,1453,1455,1454,1455,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1455,1455,1454,1456,1455,1456,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1456,1456,1455,1457,1456,1457,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1457,1457,1456,1458,1457,1458,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1458,1458,1457,1459,1458,1459,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1459,1459,1458,1460,1459,1460,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1460,1460,1459,1461,1460,1461,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1461,1461,1460,1462,1461,1462,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1462,1462,1461,1463,1462,1463,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1463,1463,1462,1464,1463,1464,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1464,1464,1463,1465,1464,1465,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1465,1465,1464,1466,1465,1466,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1466,1466,1465,1467,1466,1467,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1467,1467,1466,1468,1467,1468,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1468,1468,1467,1469,1468,1469,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1469,1469,1468,1470,1469,1470,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1470,1470,1469,1471,1470,1471,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1471,1471,1470,1472,1471,1472,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1472,1472,1471,1473,1472,1473,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1473,1473,1472,1474,1473,1474,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1474,1474,1473,1475,1474,1475,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1475,1475,1474,1476,1475,1476,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1476,1476,1475,1477,1476,1477,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1477,1477,1476,1478,1477,1478,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1478,1478,1477,1479,1478,1479,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1479,1479,1478,1480,1479,1480,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1480,1480,1479,1481,1480,1481,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1481,1481,1480,1482,1481,1482,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1482,1482,1481,1483,1482,1483,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1483,1483,1482,1484,1483,1484,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1484,1484,1483,1485,1484,1485,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1485,1485,1484,1486,1485,1486,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1486,1486,1485,1487,1486,1487,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1487,1487,1486,1488,1487,1488,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1488,1488,1487,1489,1488,1489,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1489,1489,1488,1490,1489,1490,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1490,1490,1489,1491,1490,1491,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1491,1491,1490,1492,1491,1492,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1492,1492,1491,1493,1492,1493,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1493,1493,1492,1494,1493,1494,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1494,1494,1493,1495,1494,1495,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1495,1495,1494,1496,1495,1496,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1496,1496,1495,1497,1496,1497,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1497,1497,1496,1498,1497,1498,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1498,1498,1497,1499,1498,1499,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1499,1499,1498,1500,1499,1500,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1500,1500,1499,1501,1500,1501,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1501,1501,1500,1502,1501,1502,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1502,1502,1501,1503,1502,1503,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1503,1503,1502,1504,1503,1504,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1504,1504,1503,1505,1504,1505,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1505,1505,1504,1506,1505,1506,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1506,1506,1505,1507,1506,1507,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1507,1507,1506,1508,1507,1508,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1508,1508,1507,1509,1508,1509,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1509,1509,1508,1510,1509,1510,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1510,1510,1509,1511,1510,1511,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1511,1511,1510,1512,1511,1512,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1512,1512,1511,1513,1512,1513,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1513,1513,1512,1514,1513,1514,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1514,1514,1513,1515,1514,1515,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1515,1515,1514,1516,1515,1516,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1516,1516,1515,1517,1516,1517,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1517,1517,1516,1518,1517,1518,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1518,1518,1517,1519,1518,1519,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1519,1519,1518,1520,1519,1520,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1520,1520,1519,1521,1520,1521,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1521,1521,1520,1522,1521,1522,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1522,1522,1521,1523,1522,1523,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1523,1523,1522,1524,1523,1524,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1524,1524,1523,1525,1524,1525,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1525,1525,1524,1526,1525,1526,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1526,1526,1525,1527,1526,1527,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1527,1527,1526,1528,1527,1528,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1528,1528,1527,1529,1528,1529,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1529,1529,1528,1530,1529,1530,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1530,1530,1529,1531,1530,1531,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1531,1531,1530,1532,1531,1532,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1532,1532,1531,1533,1532,1533,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1533,1533,1532,1534,1533,1534,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1534,1534,1533,1535,1534,1535,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1535,1535,1534,1536,1535,1536,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1536,1536,1535,1537,1536,1537,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1537,1537,1536,0,1537,1538,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv new file mode 100644 index 00000000..6aac5521 --- /dev/null +++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv @@ -0,0 +1,539 @@ +NID,PID +1001,1001 +1002,1002 +1003,1003 +1004,1004 +1005,1005 +1006,1006 +1007,1007 +1008,1008 +1009,1009 +1010,1010 +1011,1011 +1012,1012 +1013,1013 +1014,1014 +1015,1015 +1016,1016 +1017,1017 +1018,1018 +1019,1019 +1020,1020 +1021,1021 +1022,1022 +1023,1023 +1024,1024 +1025,1025 +1026,1026 +1027,1027 +1028,1028 +1029,1029 +1030,1030 +1031,1031 +1032,1032 +1033,1033 +1034,1034 +1035,1035 +1036,1036 +1037,1037 +1038,1038 +1039,1039 +1040,1040 +1041,1041 +1042,1042 +1043,1043 +1044,1044 +1045,1045 +1046,1046 +1047,1047 +1048,1048 +1049,1049 +1050,1050 +1051,1051 +1052,1052 +1053,1053 +1054,1054 +1055,1055 +1056,1056 +1057,1057 +1058,1058 +1059,1059 +1060,1060 +1061,1061 +1062,1062 +1063,1063 +1064,1064 +1065,1065 +1066,1066 +1067,1067 +1068,1068 +1069,1069 +1070,1070 +1071,1071 +1072,1072 +1073,1073 +1074,1074 +1075,1075 +1076,1076 +1077,1077 +1078,1078 +1079,1079 +1080,1080 +1081,1081 +1082,1082 +1083,1083 +1084,1084 +1085,1085 +1086,1086 +1087,1087 +1088,1088 +1089,1089 +1090,1090 +1091,1091 +1092,1092 +1093,1093 +1094,1094 +1095,1095 +1096,1096 +1097,1097 +1098,1098 +1099,1099 +1100,1100 +1101,1101 +1102,1102 +1103,1103 +1104,1104 +1105,1105 +1106,1106 +1107,1107 +1108,1108 +1109,1109 +1110,1110 +1111,1111 +1112,1112 +1113,1113 +1114,1114 +1115,1115 +1116,1116 +1117,1117 +1118,1118 +1119,1119 +1120,1120 +1121,1121 +1122,1122 +1123,1123 +1124,1124 +1125,1125 +1126,1126 +1127,1127 +1128,1128 +1129,1129 +1130,1130 +1131,1131 +1132,1132 +1133,1133 +1134,1134 +1135,1135 +1136,1136 +1137,1137 +1138,1138 +1139,1139 +1140,1140 +1141,1141 +1142,1142 +1143,1143 +1144,1144 +1145,1145 +1146,1146 +1147,1147 +1148,1148 +1149,1149 +1150,1150 +1151,1151 +1152,1152 +1153,1153 +1154,1154 +1155,1155 +1156,1156 +1157,1157 +1158,1158 +1159,1159 +1160,1160 +1161,1161 +1162,1162 +1163,1163 +1164,1164 +1165,1165 +1166,1166 +1167,1167 +1168,1168 +1169,1169 +1170,1170 +1171,1171 +1172,1172 +1173,1173 +1174,1174 +1175,1175 +1176,1176 +1177,1177 +1178,1178 +1179,1179 +1180,1180 +1181,1181 +1182,1182 +1183,1183 +1184,1184 +1185,1185 +1186,1186 +1187,1187 +1188,1188 +1189,1189 +1190,1190 +1191,1191 +1192,1192 +1193,1193 +1194,1194 +1195,1195 +1196,1196 +1197,1197 +1198,1198 +1199,1199 +1200,1200 +1201,1201 +1202,1202 +1203,1203 +1204,1204 +1205,1205 +1206,1206 +1207,1207 +1208,1208 +1209,1209 +1210,1210 +1211,1211 +1212,1212 +1213,1213 +1214,1214 +1215,1215 +1216,1216 +1217,1217 +1218,1218 +1219,1219 +1220,1220 +1221,1221 +1222,1222 +1223,1223 +1224,1224 +1225,1225 +1226,1226 +1227,1227 +1228,1228 +1229,1229 +1230,1230 +1231,1231 +1232,1232 +1233,1233 +1234,1234 +1235,1235 +1236,1236 +1237,1237 +1238,1238 +1239,1239 +1240,1240 +1241,1241 +1242,1242 +1243,1243 +1244,1244 +1245,1245 +1246,1246 +1247,1247 +1248,1248 +1249,1249 +1250,1250 +1251,1251 +1252,1252 +1253,1253 +1254,1254 +1255,1255 +1256,1256 +1257,1257 +1258,1258 +1259,1259 +1260,1260 +1261,1261 +1262,1262 +1263,1263 +1264,1264 +1265,1265 +1266,1266 +1267,1267 +1268,1268 +1269,1269 +1270,1270 +1271,1271 +1272,1272 +1273,1273 +1274,1274 +1275,1275 +1276,1276 +1277,1277 +1278,1278 +1279,1279 +1280,1280 +1281,1281 +1282,1282 +1283,1283 +1284,1284 +1285,1285 +1286,1286 +1287,1287 +1288,1288 +1289,1289 +1290,1290 +1291,1291 +1292,1292 +1293,1293 +1294,1294 +1295,1295 +1296,1296 +1297,1297 +1298,1298 +1299,1299 +1300,1300 +1301,1301 +1302,1302 +1303,1303 +1304,1304 +1305,1305 +1306,1306 +1307,1307 +1308,1308 +1309,1309 +1310,1310 +1311,1311 +1312,1312 +1313,1313 +1314,1314 +1315,1315 +1316,1316 +1317,1317 +1318,1318 +1319,1319 +1320,1320 +1321,1321 +1322,1322 +1323,1323 +1324,1324 +1325,1325 +1326,1326 +1327,1327 +1328,1328 +1329,1329 +1330,1330 +1331,1331 +1332,1332 +1333,1333 +1334,1334 +1335,1335 +1336,1336 +1337,1337 +1338,1338 +1339,1339 +1340,1340 +1341,1341 +1342,1342 +1343,1343 +1344,1344 +1345,1345 +1346,1346 +1347,1347 +1348,1348 +1349,1349 +1350,1350 +1351,1351 +1352,1352 +1353,1353 +1354,1354 +1355,1355 +1356,1356 +1357,1357 +1358,1358 +1359,1359 +1360,1360 +1361,1361 +1362,1362 +1363,1363 +1364,1364 +1365,1365 +1366,1366 +1367,1367 +1368,1368 +1369,1369 +1370,1370 +1371,1371 +1372,1372 +1373,1373 +1374,1374 +1375,1375 +1376,1376 +1377,1377 +1378,1378 +1379,1379 +1380,1380 +1381,1381 +1382,1382 +1383,1383 +1384,1384 +1385,1385 +1386,1386 +1387,1387 +1388,1388 +1389,1389 +1390,1390 +1391,1391 +1392,1392 +1393,1393 +1394,1394 +1395,1395 +1396,1396 +1397,1397 +1398,1398 +1399,1399 +1400,1400 +1401,1401 +1402,1402 +1403,1403 +1404,1404 +1405,1405 +1406,1406 +1407,1407 +1408,1408 +1409,1409 +1410,1410 +1411,1411 +1412,1412 +1413,1413 +1414,1414 +1415,1415 +1416,1416 +1417,1417 +1418,1418 +1419,1419 +1420,1420 +1421,1421 +1422,1422 +1423,1423 +1424,1424 +1425,1425 +1426,1426 +1427,1427 +1428,1428 +1429,1429 +1430,1430 +1431,1431 +1432,1432 +1433,1433 +1434,1434 +1435,1435 +1436,1436 +1437,1437 +1438,1438 +1439,1439 +1440,1440 +1441,1441 +1442,1442 +1443,1443 +1444,1444 +1445,1445 +1446,1446 +1447,1447 +1448,1448 +1449,1449 +1450,1450 +1451,1451 +1452,1452 +1453,1453 +1454,1454 +1455,1455 +1456,1456 +1457,1457 +1458,1458 +1459,1459 +1460,1460 +1461,1461 +1462,1462 +1463,1463 +1464,1464 +1465,1465 +1466,1466 +1467,1467 +1468,1468 +1469,1469 +1470,1470 +1471,1471 +1472,1472 +1473,1473 +1474,1474 +1475,1475 +1476,1476 +1477,1477 +1478,1478 +1479,1479 +1480,1480 +1481,1481 +1482,1482 +1483,1483 +1484,1484 +1485,1485 +1486,1486 +1487,1487 +1488,1488 +1489,1489 +1490,1490 +1491,1491 +1492,1492 +1493,1493 +1494,1494 +1495,1495 +1496,1496 +1497,1497 +1498,1498 +1499,1499 +1500,1500 +1501,1501 +1502,1502 +1503,1503 +1504,1504 +1505,1505 +1506,1506 +1507,1507 +1508,1508 +1509,1509 +1510,1510 +1511,1511 +1512,1512 +1513,1513 +1514,1514 +1515,1515 +1516,1516 +1517,1517 +1518,1518 +1519,1519 +1520,1520 +1521,1521 +1522,1522 +1523,1523 +1524,1524 +1525,1525 +1526,1526 +1527,1527 +1528,1528 +1529,1529 +1530,1530 +1531,1531 +1532,1532 +1533,1533 +1534,1534 +1535,1535 +1536,1536 +1537,1537 +1538,1538 diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv new file mode 100644 index 00000000..49c7cdc7 --- /dev/null +++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv @@ -0,0 +1,539 @@ +PID,B(Lat),L(Long),H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3 +1001,0,0,0.041,129.675,314.065,0,0,0,0 +1002,0,0,0.04,130.691,314.065,0,0,0,0 +1003,0,0,0.04,131.708,314.064,0,0,0,0 +1004,0,0,0.04,132.718,314.064,0,0,0,0 +1005,0,0,0.04,133.745,314.064,0,0,0,0 +1006,0,0,0.04,134.801,314.066,0,0,0,0 +1007,0,0,0.04,135.862,314.068,0,0,0,0 +1008,0,0,0.04,136.865,314.07,0,0,0,0 +1009,0,0,0.04,137.897,314.072,0,0,0,0 +1010,0,0,0.04,138.902,314.075,0,0,0,0 +1011,0,0,0.04,139.921,314.077,0,0,0,0 +1012,0,0,0.04,140.939,314.08,0,0,0,0 +1013,0,0,0.04,141.951,314.082,0,0,0,0 +1014,0,0,0.04,142.983,314.084,0,0,0,0 +1015,0,0,0.04,144.003,314.086,0,0,0,0 +1016,0,0,0.04,145.06,314.106,0,0,0,0 +1017,0,0,0.04,146.06,314.118,0,0,0,0 +1018,0,0,0.04,147.107,314.129,0,0,0,0 +1019,0,0,0.04,148.109,314.14,0,0,0,0 +1020,0,0,0.04,149.118,314.15,0,0,0,0 +1021,0,0,0.04,150.136,314.161,0,0,0,0 +1022,0,0,0.04,151.154,314.172,0,0,0,0 +1023,0,0,0.04,152.171,314.183,0,0,0,0 +1024,0,0,0.04,153.198,314.193,0,0,0,0 +1025,0,0,0.04,154.224,314.204,0,0,0,0 +1026,0,0,0.04,155.287,314.215,0,0,0,0 +1027,0,0,0.04,156.302,314.226,0,0,0,0 +1028,0,0,0.04,157.312,314.237,0,0,0,0 +1029,0,0,0.04,158.379,314.248,0,0,0,0 +1030,0,0,0.04,159.455,314.259,0,0,0,0 +1031,0,0,0.04,160.51,314.271,0,0,0,0 +1032,0,0,0.044,161.521,314.282,0,0,0,0 +1033,0,0,0.057,162.574,314.296,0,0,0,0 +1034,0,0,0.073,163.634,314.311,0,0,0,0 +1035,0,0,0.081,164.666,314.326,0,0,0,0 +1036,0,0,0.097,165.729,314.344,0,0,0,0 +1037,0,0,0.107,166.753,314.363,0,0,0,0 +1038,0,0,0.106,167.858,314.382,0,0,0,0 +1039,0,0,0.106,168.978,314.402,0,0,0,0 +1040,0,0,0.106,170.06,314.42,0,0,0,0 +1041,0,0,0.106,171.126,314.438,0,0,0,0 +1042,0,0,0.105,172.155,314.455,0,0,0,0 +1043,0,0,0.105,173.164,314.473,0,0,0,0 +1044,0,0,0.105,174.225,314.49,0,0,0,0 +1045,0,0,0.099,175.296,314.508,0,0,0,0 +1046,0,0,0.083,176.319,314.524,0,0,0,0 +1047,0,0,0.076,177.46,314.542,0,0,0,0 +1048,0,0,0.059,178.598,314.558,0,0,0,0 +1049,0,0,0.047,179.602,314.571,0,0,0,0 +1050,0,0,0.043,180.621,314.584,0,0,0,0 +1051,0,0,0.04,181.718,314.598,0,0,0,0 +1052,0,0,0.04,182.79,314.611,0,0,0,0 +1053,0,0,0.04,183.825,314.624,0,0,0,0 +1054,0,0,0.04,184.87,314.637,0,0,0,0 +1055,0,0,0.04,185.896,314.649,0,0,0,0 +1056,0,0,0.04,187.05,314.664,0,0,0,0 +1057,0,0,0.04,188.124,314.677,0,0,0,0 +1058,0,0,0.04,189.203,314.69,0,0,0,0 +1059,0,0,0.04,190.271,314.703,0,0,0,0 +1060,0,0,0.04,191.384,314.717,0,0,0,0 +1061,0,0,0.04,192.396,314.729,0,0,0,0 +1062,0,0,0.04,193.474,314.743,0,0,0,0 +1063,0,0,0.04,194.501,314.756,0,0,0,0 +1064,0,0,0.04,195.517,314.768,0,0,0,0 +1065,0,0,0.04,196.577,314.781,0,0,0,0 +1066,0,0,0.04,197.587,314.794,0,0,0,0 +1067,0,0,0.04,198.625,314.806,0,0,0,0 +1068,0,0,0.04,199.67,314.819,0,0,0,0 +1069,0,0,0.04,200.715,314.832,0,0,0,0 +1070,0,0,0.04,201.723,314.844,0,0,0,0 +1071,0,0,0.04,202.797,314.858,0,0,0,0 +1072,0,0,0.04,203.836,314.871,0,0,0,0 +1073,0,0,0.04,204.877,314.884,0,0,0,0 +1074,0,0,0.04,205.9,314.896,0,0,0,0 +1075,0,0,0.04,206.914,314.908,0,0,0,0 +1076,0,0,0.04,207.922,314.92,0,0,0,0 +1077,0,0,0.04,208.936,314.931,0,0,0,0 +1078,0,0,0.04,209.952,314.943,0,0,0,0 +1079,0,0,0.04,210.974,314.955,0,0,0,0 +1080,0,0,0.04,211.985,314.966,0,0,0,0 +1081,0,0,0.04,213.011,314.978,0,0,0,0 +1082,0,0,0.04,214.037,314.99,0,0,0,0 +1083,0,0,0.04,215.109,315.002,0,0,0,0 +1084,0,0,0.04,216.203,315.014,0,0,0,0 +1085,0,0,0.04,217.212,315.026,0,0,0,0 +1086,0,0,0.04,218.226,315.038,0,0,0,0 +1087,0,0,0.04,219.321,315.05,0,0,0,0 +1088,0,0,0.04,220.33,315.062,0,0,0,0 +1089,0,0,0.04,221.382,315.074,0,0,0,0 +1090,0,0,0.04,222.454,315.086,0,0,0,0 +1091,0,0,0.04,223.615,315.099,0,0,0,0 +1092,0,0,0.04,224.672,315.111,0,0,0,0 +1093,0,0,0.04,225.705,315.123,0,0,0,0 +1094,0,0,0.04,226.826,315.136,0,0,0,0 +1095,0,0,0.04,227.872,315.148,0,0,0,0 +1096,0,0,0.04,228.885,315.16,0,0,0,0 +1097,0,0,0.04,230.0,315.173,0,0,0,0 +1098,0,0,0.04,231.02,315.184,0,0,0,0 +1099,0,0,0.04,232.043,315.189,0,0,0,0 +1100,0,0,0.04,233.113,315.197,0,0,0,0 +1101,0,0,0.04,234.143,315.205,0,0,0,0 +1102,0,0,0.04,235.147,315.214,0,0,0,0 +1103,0,0,0.04,236.19,315.223,0,0,0,0 +1104,0,0,0.04,237.216,315.232,0,0,0,0 +1105,0,0,0.04,238.337,315.243,0,0,0,0 +1106,0,0,0.04,239.383,315.253,0,0,0,0 +1107,0,0,0.04,240.532,315.263,0,0,0,0 +1108,0,0,0.04,241.549,315.273,0,0,0,0 +1109,0,0,0.04,242.63,315.283,0,0,0,0 +1110,0,0,0.04,243.672,315.293,0,0,0,0 +1111,0,0,0.04,244.725,315.303,0,0,0,0 +1112,0,0,0.04,245.757,315.312,0,0,0,0 +1113,0,0,0.04,246.832,315.322,0,0,0,0 +1114,0,0,0.04,247.849,315.332,0,0,0,0 +1115,0,0,0.04,248.861,315.341,0,0,0,0 +1116,0,0,0.04,249.983,315.351,0,0,0,0 +1117,0,0,0.04,251.03,315.36,0,0,0,0 +1118,0,0,0.04,252.044,315.369,0,0,0,0 +1119,0,0,0.04,253.082,315.379,0,0,0,0 +1120,0,0,0.04,254.086,315.388,0,0,0,0 +1121,0,0,0.04,255.094,315.396,0,0,0,0 +1122,0,0,0.04,256.112,315.406,0,0,0,0 +1123,0,0,0.04,257.132,315.415,0,0,0,0 +1124,0,0,0.04,258.155,315.424,0,0,0,0 +1125,0,0,0.04,259.207,315.434,0,0,0,0 +1126,0,0,0.04,260.216,315.443,0,0,0,0 +1127,0,0,0.04,261.256,315.451,0,0,0,0 +1128,0,0,0.04,262.261,315.46,0,0,0,0 +1129,0,0,0.04,263.277,315.468,0,0,0,0 +1130,0,0,0.04,264.326,315.477,0,0,0,0 +1131,0,0,0.04,265.345,315.486,0,0,0,0 +1132,0,0,0.04,266.411,315.495,0,0,0,0 +1133,0,0,0.04,267.451,315.489,0,0,0,0 +1134,0,0,0.04,268.476,315.468,0,0,0,0 +1135,0,0,0.04,269.535,315.413,0,0,0,0 +1136,0,0,0.04,270.531,315.265,0,0,0,0 +1137,0,0,0.04,271.547,315.158,0,0,0,0 +1138,0,0,0.04,272.559,315.05,0,0,0,0 +1139,0,0,0.04,273.578,314.942,0,0,0,0 +1140,0,0,0.04,274.575,314.835,0,0,0,0 +1141,0,0,0.04,275.579,314.715,0,0,0,0 +1142,0,0,0.04,276.575,314.54,0,0,0,0 +1143,0,0,0.04,277.614,314.381,0,0,0,0 +1144,0,0,0.04,278.601,314.204,0,0,0,0 +1145,0,0,0.04,279.581,313.96,0,0,0,0 +1146,0,0,0.04,280.56,313.714,0,0,0,0 +1147,0,0,0.04,281.543,313.481,0,0,0,0 +1148,0,0,0.04,282.527,313.248,0,0,0,0 +1149,0,0,0.04,283.519,313.013,0,0,0,0 +1150,0,0,0.04,284.52,312.746,0,0,0,0 +1151,0,0,0.04,285.475,312.445,0,0,0,0 +1152,0,0,0.04,286.412,312.09,0,0,0,0 +1153,0,0,0.04,287.373,311.745,0,0,0,0 +1154,0,0,0.04,288.343,311.409,0,0,0,0 +1155,0,0,0.04,289.274,311.0,0,0,0,0 +1156,0,0,0.04,290.205,310.624,0,0,0,0 +1157,0,0,0.04,291.159,310.24,0,0,0,0 +1158,0,0,0.04,292.084,309.796,0,0,0,0 +1159,0,0,0.04,293.016,309.367,0,0,0,0 +1160,0,0,0.04,293.971,308.908,0,0,0,0 +1161,0,0,0.04,294.88,308.483,0,0,0,0 +1162,0,0,0.04,295.784,308.018,0,0,0,0 +1163,0,0,0.04,296.743,307.478,0,0,0,0 +1164,0,0,0.04,297.636,306.93,0,0,0,0 +1165,0,0,0.04,298.498,306.347,0,0,0,0 +1166,0,0,0.04,299.335,305.787,0,0,0,0 +1167,0,0,0.04,300.13,305.165,0,0,0,0 +1168,0,0,0.04,300.874,304.497,0,0,0,0 +1169,0,0,0.04,301.67,303.856,0,0,0,0 +1170,0,0,0.04,302.439,303.194,0,0,0,0 +1171,0,0,0.04,303.161,302.486,0,0,0,0 +1172,0,0,0.04,303.779,301.7,0,0,0,0 +1173,0,0,0.04,304.384,300.861,0,0,0,0 +1174,0,0,0.04,304.956,300.033,0,0,0,0 +1175,0,0,0.04,305.533,299.183,0,0,0,0 +1176,0,0,0.04,306.09,298.344,0,0,0,0 +1177,0,0,0.04,306.609,297.475,0,0,0,0 +1178,0,0,0.04,307.14,296.601,0,0,0,0 +1179,0,0,0.04,307.616,295.704,0,0,0,0 +1180,0,0,0.04,307.944,294.752,0,0,0,0 +1181,0,0,0.04,308.317,293.808,0,0,0,0 +1182,0,0,0.04,308.686,292.858,0,0,0,0 +1183,0,0,0.04,308.984,291.883,0,0,0,0 +1184,0,0,0.04,309.315,290.926,0,0,0,0 +1185,0,0,0.04,309.589,289.947,0,0,0,0 +1186,0,0,0.04,309.848,288.964,0,0,0,0 +1187,0,0,0.04,309.999,287.97,0,0,0,0 +1188,0,0,0.04,310.177,286.946,0,0,0,0 +1189,0,0,0.04,310.283,285.926,0,0,0,0 +1190,0,0,0.04,310.371,284.912,0,0,0,0 +1191,0,0,0.04,310.447,283.908,0,0,0,0 +1192,0,0,0.04,310.508,282.891,0,0,0,0 +1193,0,0,0.04,310.559,281.88,0,0,0,0 +1194,0,0,0.04,310.597,280.847,0,0,0,0 +1195,0,0,0.04,310.647,279.829,0,0,0,0 +1196,0,0,0.04,310.697,278.789,0,0,0,0 +1197,0,0,0.04,310.745,277.778,0,0,0,0 +1198,0,0,0.04,310.781,276.772,0,0,0,0 +1199,0,0,0.04,310.795,275.754,0,0,0,0 +1200,0,0,0.04,310.794,274.732,0,0,0,0 +1201,0,0,0.04,310.806,273.729,0,0,0,0 +1202,0,0,0.04,310.819,272.663,0,0,0,0 +1203,0,0,0.04,310.831,271.638,0,0,0,0 +1204,0,0,0.04,310.843,270.601,0,0,0,0 +1205,0,0,0.04,310.854,269.569,0,0,0,0 +1206,0,0,0.04,310.866,268.519,0,0,0,0 +1207,0,0,0.04,310.878,267.47,0,0,0,0 +1208,0,0,0.04,310.889,266.455,0,0,0,0 +1209,0,0,0.04,310.901,265.416,0,0,0,0 +1210,0,0,0.04,310.913,264.382,0,0,0,0 +1211,0,0,0.04,310.924,263.362,0,0,0,0 +1212,0,0,0.041,310.936,262.355,0,0,0,0 +1213,0,0,0.041,310.948,261.253,0,0,0,0 +1214,0,0,0.041,310.961,260.185,0,0,0,0 +1215,0,0,0.042,310.973,259.12,0,0,0,0 +1216,0,0,0.042,310.985,258.089,0,0,0,0 +1217,0,0,0.042,310.997,257.076,0,0,0,0 +1218,0,0,0.042,311.009,256.063,0,0,0,0 +1219,0,0,0.042,311.022,255.038,0,0,0,0 +1220,0,0,0.042,311.034,254.01,0,0,0,0 +1221,0,0,0.041,311.046,252.982,0,0,0,0 +1222,0,0,0.041,311.058,251.978,0,0,0,0 +1223,0,0,0.04,311.07,250.92,0,0,0,0 +1224,0,0,0.04,311.081,249.888,0,0,0,0 +1225,0,0,0.04,311.091,248.882,0,0,0,0 +1226,0,0,0.04,311.101,247.881,0,0,0,0 +1227,0,0,0.04,311.11,246.835,0,0,0,0 +1228,0,0,0.04,311.12,245.793,0,0,0,0 +1229,0,0,0.04,311.129,244.762,0,0,0,0 +1230,0,0,0.04,311.138,243.746,0,0,0,0 +1231,0,0,0.04,311.147,242.739,0,0,0,0 +1232,0,0,0.04,311.156,241.721,0,0,0,0 +1233,0,0,0.04,311.164,240.714,0,0,0,0 +1234,0,0,0.04,311.174,239.674,0,0,0,0 +1235,0,0,0.04,311.184,238.671,0,0,0,0 +1236,0,0,0.04,311.193,237.614,0,0,0,0 +1237,0,0,0.04,311.202,236.601,0,0,0,0 +1238,0,0,0.04,311.211,235.6,0,0,0,0 +1239,0,0,0.04,311.221,234.531,0,0,0,0 +1240,0,0,0.04,311.23,233.529,0,0,0,0 +1241,0,0,0.04,311.239,232.499,0,0,0,0 +1242,0,0,0.04,311.248,231.437,0,0,0,0 +1243,0,0,0.04,311.257,230.436,0,0,0,0 +1244,0,0,0.04,311.267,229.406,0,0,0,0 +1245,0,0,0.04,311.276,228.387,0,0,0,0 +1246,0,0,0.04,311.285,227.348,0,0,0,0 +1247,0,0,0.04,311.294,226.335,0,0,0,0 +1248,0,0,0.04,311.303,225.271,0,0,0,0 +1249,0,0,0.04,311.311,224.217,0,0,0,0 +1250,0,0,0.04,311.319,223.18,0,0,0,0 +1251,0,0,0.04,311.327,222.147,0,0,0,0 +1252,0,0,0.04,311.335,221.081,0,0,0,0 +1253,0,0,0.04,311.342,220.077,0,0,0,0 +1254,0,0,0.04,311.349,219.047,0,0,0,0 +1255,0,0,0.04,311.355,218.017,0,0,0,0 +1256,0,0,0.04,311.36,216.998,0,0,0,0 +1257,0,0,0.04,311.365,215.981,0,0,0,0 +1258,0,0,0.04,311.369,214.965,0,0,0,0 +1259,0,0,0.04,311.373,213.941,0,0,0,0 +1260,0,0,0.04,311.343,212.913,0,0,0,0 +1261,0,0,0.042,311.248,211.906,0,0,0,0 +1262,0,0,0.043,311.187,210.897,0,0,0,0 +1263,0,0,0.044,311.046,209.899,0,0,0,0 +1264,0,0,0.048,310.824,208.922,0,0,0,0 +1265,0,0,0.05,310.516,207.949,0,0,0,0 +1266,0,0,0.052,310.185,207.001,0,0,0,0 +1267,0,0,0.055,309.748,206.095,0,0,0,0 +1268,0,0,0.057,309.177,205.27,0,0,0,0 +1269,0,0,0.059,308.593,204.446,0,0,0,0 +1270,0,0,0.06,307.926,203.69,0,0,0,0 +1271,0,0,0.06,307.133,203.067,0,0,0,0 +1272,0,0,0.058,306.226,202.628,0,0,0,0 +1273,0,0,0.057,305.273,202.298,0,0,0,0 +1274,0,0,0.056,304.284,202.02,0,0,0,0 +1275,0,0,0.056,303.281,201.874,0,0,0,0 +1276,0,0,0.055,302.284,201.781,0,0,0,0 +1277,0,0,0.055,301.277,201.779,0,0,0,0 +1278,0,0,0.055,300.249,201.769,0,0,0,0 +1279,0,0,0.055,299.234,201.716,0,0,0,0 +1280,0,0,0.055,298.159,201.666,0,0,0,0 +1281,0,0,0.055,297.145,201.589,0,0,0,0 +1282,0,0,0.055,296.147,201.529,0,0,0,0 +1283,0,0,0.055,295.121,201.467,0,0,0,0 +1284,0,0,0.055,294.114,201.442,0,0,0,0 +1285,0,0,0.055,293.11,201.403,0,0,0,0 +1286,0,0,0.055,292.083,201.374,0,0,0,0 +1287,0,0,0.055,291.079,201.34,0,0,0,0 +1288,0,0,0.055,290.072,201.307,0,0,0,0 +1289,0,0,0.055,289.048,201.274,0,0,0,0 +1290,0,0,0.055,288.04,201.259,0,0,0,0 +1291,0,0,0.055,287.022,201.236,0,0,0,0 +1292,0,0,0.055,286.012,201.214,0,0,0,0 +1293,0,0,0.055,285.001,201.192,0,0,0,0 +1294,0,0,0.055,283.967,201.169,0,0,0,0 +1295,0,0,0.055,282.912,201.145,0,0,0,0 +1296,0,0,0.055,281.895,201.122,0,0,0,0 +1297,0,0,0.055,280.838,201.097,0,0,0,0 +1298,0,0,0.055,279.786,201.073,0,0,0,0 +1299,0,0,0.055,278.779,201.05,0,0,0,0 +1300,0,0,0.055,277.703,201.026,0,0,0,0 +1301,0,0,0.055,276.592,201.001,0,0,0,0 +1302,0,0,0.055,275.544,200.977,0,0,0,0 +1303,0,0,0.055,274.516,200.954,0,0,0,0 +1304,0,0,0.055,273.489,200.931,0,0,0,0 +1305,0,0,0.055,272.469,200.908,0,0,0,0 +1306,0,0,0.055,271.469,200.885,0,0,0,0 +1307,0,0,0.055,270.465,200.862,0,0,0,0 +1308,0,0,0.055,269.413,200.839,0,0,0,0 +1309,0,0,0.055,268.395,200.816,0,0,0,0 +1310,0,0,0.055,267.356,200.793,0,0,0,0 +1311,0,0,0.055,266.344,200.77,0,0,0,0 +1312,0,0,0.055,265.306,200.747,0,0,0,0 +1313,0,0,0.055,264.206,200.722,0,0,0,0 +1314,0,0,0.055,263.138,200.698,0,0,0,0 +1315,0,0,0.055,262.101,200.675,0,0,0,0 +1316,0,0,0.055,261.081,200.652,0,0,0,0 +1317,0,0,0.055,259.992,200.628,0,0,0,0 +1318,0,0,0.055,258.808,200.601,0,0,0,0 +1319,0,0,0.055,257.639,200.575,0,0,0,0 +1320,0,0,0.055,256.545,200.55,0,0,0,0 +1321,0,0,0.055,255.51,200.527,0,0,0,0 +1322,0,0,0.057,254.446,200.503,0,0,0,0 +1323,0,0,0.057,253.43,200.48,0,0,0,0 +1324,0,0,0.057,252.377,200.457,0,0,0,0 +1325,0,0,0.058,251.305,200.433,0,0,0,0 +1326,0,0,0.059,250.233,200.41,0,0,0,0 +1327,0,0,0.06,249.195,200.387,0,0,0,0 +1328,0,0,0.06,248.111,200.363,0,0,0,0 +1329,0,0,0.06,247.042,200.333,0,0,0,0 +1330,0,0,0.061,245.893,200.304,0,0,0,0 +1331,0,0,0.06,244.748,200.275,0,0,0,0 +1332,0,0,0.06,243.614,200.247,0,0,0,0 +1333,0,0,0.059,242.603,200.221,0,0,0,0 +1334,0,0,0.058,241.58,200.195,0,0,0,0 +1335,0,0,0.057,240.545,200.168,0,0,0,0 +1336,0,0,0.056,239.492,200.142,0,0,0,0 +1337,0,0,0.055,238.416,200.114,0,0,0,0 +1338,0,0,0.055,237.335,200.086,0,0,0,0 +1339,0,0,0.055,236.265,200.059,0,0,0,0 +1340,0,0,0.055,235.193,200.031,0,0,0,0 +1341,0,0,0.055,234.171,200.007,0,0,0,0 +1342,0,0,0.055,233.159,200.009,0,0,0,0 +1343,0,0,0.055,232.016,199.995,0,0,0,0 +1344,0,0,0.055,231.001,199.965,0,0,0,0 +1345,0,0,0.055,229.934,199.938,0,0,0,0 +1346,0,0,0.055,228.932,199.913,0,0,0,0 +1347,0,0,0.055,227.91,199.888,0,0,0,0 +1348,0,0,0.055,226.885,199.863,0,0,0,0 +1349,0,0,0.055,225.858,199.838,0,0,0,0 +1350,0,0,0.055,224.852,199.813,0,0,0,0 +1351,0,0,0.055,223.798,199.787,0,0,0,0 +1352,0,0,0.055,222.759,199.761,0,0,0,0 +1353,0,0,0.055,221.686,199.734,0,0,0,0 +1354,0,0,0.055,220.682,199.709,0,0,0,0 +1355,0,0,0.055,219.634,199.684,0,0,0,0 +1356,0,0,0.055,218.576,199.658,0,0,0,0 +1357,0,0,0.055,217.558,199.632,0,0,0,0 +1358,0,0,0.055,216.538,199.607,0,0,0,0 +1359,0,0,0.055,215.439,199.58,0,0,0,0 +1360,0,0,0.055,214.308,199.552,0,0,0,0 +1361,0,0,0.055,213.153,199.524,0,0,0,0 +1362,0,0,0.055,212.125,199.498,0,0,0,0 +1363,0,0,0.055,211.068,199.472,0,0,0,0 +1364,0,0,0.055,210.007,199.446,0,0,0,0 +1365,0,0,0.055,208.957,199.42,0,0,0,0 +1366,0,0,0.055,207.931,199.395,0,0,0,0 +1367,0,0,0.055,206.88,199.369,0,0,0,0 +1368,0,0,0.055,205.839,199.343,0,0,0,0 +1369,0,0,0.055,204.773,199.317,0,0,0,0 +1370,0,0,0.055,203.682,199.29,0,0,0,0 +1371,0,0,0.055,202.545,199.261,0,0,0,0 +1372,0,0,0.055,201.505,199.236,0,0,0,0 +1373,0,0,0.055,200.451,199.21,0,0,0,0 +1374,0,0,0.055,199.332,199.182,0,0,0,0 +1375,0,0,0.055,198.283,199.156,0,0,0,0 +1376,0,0,0.055,197.186,199.129,0,0,0,0 +1377,0,0,0.055,196.108,199.102,0,0,0,0 +1378,0,0,0.055,195.103,199.077,0,0,0,0 +1379,0,0,0.055,194.052,199.051,0,0,0,0 +1380,0,0,0.055,193.036,199.026,0,0,0,0 +1381,0,0,0.055,191.994,199.001,0,0,0,0 +1382,0,0,0.055,190.904,198.974,0,0,0,0 +1383,0,0,0.055,189.806,198.946,0,0,0,0 +1384,0,0,0.055,188.77,198.921,0,0,0,0 +1385,0,0,0.055,187.678,198.894,0,0,0,0 +1386,0,0,0.055,186.563,198.866,0,0,0,0 +1387,0,0,0.055,185.384,198.837,0,0,0,0 +1388,0,0,0.055,184.288,198.81,0,0,0,0 +1389,0,0,0.055,183.269,198.785,0,0,0,0 +1390,0,0,0.055,182.198,198.758,0,0,0,0 +1391,0,0,0.055,181.154,198.733,0,0,0,0 +1392,0,0,0.055,180.045,198.705,0,0,0,0 +1393,0,0,0.061,179.011,198.68,0,0,0,0 +1394,0,0,0.084,177.98,198.654,0,0,0,0 +1395,0,0,0.081,176.959,198.63,0,0,0,0 +1396,0,0,0.09,175.942,198.605,0,0,0,0 +1397,0,0,0.11,174.916,198.582,0,0,0,0 +1398,0,0,0.105,173.805,198.555,0,0,0,0 +1399,0,0,0.103,172.759,198.531,0,0,0,0 +1400,0,0,0.1,171.697,198.506,0,0,0,0 +1401,0,0,0.098,170.696,198.482,0,0,0,0 +1402,0,0,0.09,169.636,198.456,0,0,0,0 +1403,0,0,0.082,168.595,198.43,0,0,0,0 +1404,0,0,0.075,167.576,198.405,0,0,0,0 +1405,0,0,0.066,166.532,198.391,0,0,0,0 +1406,0,0,0.063,165.489,198.374,0,0,0,0 +1407,0,0,0.059,164.481,198.356,0,0,0,0 +1408,0,0,0.055,163.456,198.4,0,0,0,0 +1409,0,0,0.055,162.455,198.412,0,0,0,0 +1410,0,0,0.055,161.443,198.46,0,0,0,0 +1411,0,0,0.055,160.439,198.491,0,0,0,0 +1412,0,0,0.055,159.431,198.523,0,0,0,0 +1413,0,0,0.055,158.375,198.558,0,0,0,0 +1414,0,0,0.055,157.338,198.664,0,0,0,0 +1415,0,0,0.055,156.273,198.732,0,0,0,0 +1416,0,0,0.055,155.231,198.843,0,0,0,0 +1417,0,0,0.055,154.229,198.947,0,0,0,0 +1418,0,0,0.055,153.232,199.058,0,0,0,0 +1419,0,0,0.055,152.225,199.232,0,0,0,0 +1420,0,0,0.055,151.238,199.404,0,0,0,0 +1421,0,0,0.055,150.264,199.651,0,0,0,0 +1422,0,0,0.055,149.296,199.938,0,0,0,0 +1423,0,0,0.055,148.332,200.217,0,0,0,0 +1424,0,0,0.055,147.376,200.528,0,0,0,0 +1425,0,0,0.055,146.404,200.869,0,0,0,0 +1426,0,0,0.055,145.442,201.226,0,0,0,0 +1427,0,0,0.055,144.477,201.566,0,0,0,0 +1428,0,0,0.055,143.488,201.937,0,0,0,0 +1429,0,0,0.055,142.551,202.319,0,0,0,0 +1430,0,0,0.055,141.609,202.686,0,0,0,0 +1431,0,0,0.055,140.712,203.159,0,0,0,0 +1432,0,0,0.055,139.799,203.594,0,0,0,0 +1433,0,0,0.055,138.893,204.081,0,0,0,0 +1434,0,0,0.055,137.998,204.569,0,0,0,0 +1435,0,0,0.055,137.096,205.05,0,0,0,0 +1436,0,0,0.055,136.24,205.594,0,0,0,0 +1437,0,0,0.055,135.408,206.148,0,0,0,0 +1438,0,0,0.055,134.558,206.705,0,0,0,0 +1439,0,0,0.055,133.735,207.296,0,0,0,0 +1440,0,0,0.055,132.948,207.925,0,0,0,0 +1441,0,0,0.055,132.133,208.534,0,0,0,0 +1442,0,0,0.055,131.312,209.148,0,0,0,0 +1443,0,0,0.055,130.509,209.833,0,0,0,0 +1444,0,0,0.055,129.781,210.533,0,0,0,0 +1445,0,0,0.055,129.016,211.238,0,0,0,0 +1446,0,0,0.055,128.248,211.933,0,0,0,0 +1447,0,0,0.055,127.482,212.625,0,0,0,0 +1448,0,0,0.055,126.806,213.379,0,0,0,0 +1449,0,0,0.055,126.041,214.151,0,0,0,0 +1450,0,0,0.055,125.374,214.961,0,0,0,0 +1451,0,0,0.055,124.817,215.813,0,0,0,0 +1452,0,0,0.055,124.342,216.698,0,0,0,0 +1453,0,0,0.055,123.913,217.627,0,0,0,0 +1454,0,0,0.055,123.429,218.518,0,0,0,0 +1455,0,0,0.055,123.013,219.457,0,0,0,0 +1456,0,0,0.055,122.572,220.373,0,0,0,0 +1457,0,0,0.055,122.12,221.308,0,0,0,0 +1458,0,0,0.055,121.754,222.24,0,0,0,0 +1459,0,0,0.055,121.354,223.191,0,0,0,0 +1460,0,0,0.055,120.995,224.137,0,0,0,0 +1461,0,0,0.055,120.681,225.09,0,0,0,0 +1462,0,0,0.055,120.406,226.097,0,0,0,0 +1463,0,0,0.055,120.176,227.112,0,0,0,0 +1464,0,0,0.055,120.014,228.123,0,0,0,0 +1465,0,0,0.055,119.842,229.165,0,0,0,0 +1466,0,0,0.055,119.679,230.222,0,0,0,0 +1467,0,0,0.055,119.573,231.253,0,0,0,0 +1468,0,0,0.055,119.518,232.302,0,0,0,0 +1469,0,0,0.055,119.498,233.305,0,0,0,0 +1470,0,0,0.055,119.405,234.333,0,0,0,0 +1471,0,0,0.055,119.316,235.356,0,0,0,0 +1472,0,0,0.055,119.239,236.385,0,0,0,0 +1473,0,0,0.055,119.177,237.395,0,0,0,0 +1474,0,0,0.055,119.15,238.4,0,0,0,0 +1475,0,0,0.055,119.103,239.416,0,0,0,0 +1476,0,0,0.055,119.054,240.444,0,0,0,0 +1477,0,0,0.055,119.007,241.464,0,0,0,0 +1478,0,0,0.055,118.96,242.486,0,0,0,0 +1479,0,0,0.055,118.92,243.524,0,0,0,0 +1480,0,0,0.055,118.907,244.542,0,0,0,0 +1481,0,0,0.055,118.879,245.583,0,0,0,0 +1482,0,0,0.055,118.852,246.618,0,0,0,0 +1483,0,0,0.055,118.837,247.64,0,0,0,0 +1484,0,0,0.055,118.823,248.651,0,0,0,0 +1485,0,0,0.055,118.807,249.659,0,0,0,0 +1486,0,0,0.055,118.821,250.691,0,0,0,0 +1487,0,0,0.055,118.822,251.724,0,0,0,0 +1488,0,0,0.055,118.823,252.77,0,0,0,0 +1489,0,0,0.055,118.825,253.772,0,0,0,0 +1490,0,0,0.055,118.827,254.798,0,0,0,0 +1491,0,0,0.055,118.829,255.804,0,0,0,0 +1492,0,0,0.055,118.83,256.839,0,0,0,0 +1493,0,0,0.055,118.832,257.845,0,0,0,0 +1494,0,0,0.055,118.823,258.898,0,0,0,0 +1495,0,0,0.055,118.817,259.937,0,0,0,0 +1496,0,0,0.055,118.812,260.966,0,0,0,0 +1497,0,0,0.055,118.806,261.979,0,0,0,0 +1498,0,0,0.055,118.801,262.987,0,0,0,0 +1499,0,0,0.055,118.795,263.994,0,0,0,0 +1500,0,0,0.055,118.789,265.071,0,0,0,0 +1501,0,0,0.055,118.771,266.232,0,0,0,0 +1502,0,0,0.055,118.756,267.27,0,0,0,0 +1503,0,0,0.055,118.741,268.31,0,0,0,0 +1504,0,0,0.055,118.726,269.376,0,0,0,0 +1505,0,0,0.055,118.711,270.427,0,0,0,0 +1506,0,0,0.055,118.697,271.486,0,0,0,0 +1507,0,0,0.055,118.682,272.544,0,0,0,0 +1508,0,0,0.055,118.65,273.584,0,0,0,0 +1509,0,0,0.055,118.624,274.625,0,0,0,0 +1510,0,0,0.055,118.57,275.636,0,0,0,0 +1511,0,0,0.055,118.526,276.675,0,0,0,0 +1512,0,0,0.055,118.482,277.694,0,0,0,0 +1513,0,0,0.055,118.437,278.698,0,0,0,0 +1514,0,0,0.055,118.393,279.707,0,0,0,0 +1515,0,0,0.055,118.347,280.73,0,0,0,0 +1516,0,0,0.055,118.303,281.738,0,0,0,0 +1517,0,0,0.055,118.257,282.757,0,0,0,0 +1518,0,0,0.055,118.239,283.759,0,0,0,0 +1519,0,0,0.055,118.21,284.764,0,0,0,0 +1520,0,0,0.055,118.221,285.79,0,0,0,0 +1521,0,0,0.055,118.21,286.829,0,0,0,0 +1522,0,0,0.055,118.199,287.845,0,0,0,0 +1523,0,0,0.055,118.189,288.869,0,0,0,0 +1524,0,0,0.055,118.178,289.948,0,0,0,0 +1525,0,0,0.055,118.167,290.972,0,0,0,0 +1526,0,0,0.055,118.156,292.054,0,0,0,0 +1527,0,0,0.055,118.145,293.133,0,0,0,0 +1528,0,0,0.055,118.126,294.186,0,0,0,0 +1529,0,0,0.055,118.099,295.187,0,0,0,0 +1530,0,0,0.055,118.078,296.198,0,0,0,0 +1531,0,0,0.055,118.056,297.239,0,0,0,0 +1532,0,0,0.055,118.035,298.264,0,0,0,0 +1533,0,0,0.055,118.014,299.268,0,0,0,0 +1534,0,0,0.055,117.991,300.327,0,0,0,0 +1535,0,0,0.055,117.971,301.328,0,0,0,0 +1536,0,0,0.055,117.948,302.415,0,0,0,0 +1537,0,0,0.055,117.927,303.433,0,0,0,0 +1538,0,0,0.055,117.907,304.449,0,0,0,0 diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv new file mode 100644 index 00000000..bdbe3cb0 --- /dev/null +++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv @@ -0,0 +1,160 @@ +DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW +1001,0,1001,1.5769552072461928,0,90000000000,0,0,2,2 +1002,1,1002,1.5769552072461928,0,90000000000,0,0,2,2 +1003,2,1003,1.5737316285471363,0,-310.21423497205427,0,0,2,2 +1004,3,1004,1.5725114416586194,0,-819.5465870113417,0,0,2,2 +1005,4,1005,1.57844374331971,0,168.5686361094724,0,0,2,2 +1006,5,1006,1.5663694826442065,0,-82.8208059172365,0,0,2,2 +1007,6,1007,1.5733395387307152,0,143.47086846770284,0,0,2,2 +1008,7,1008,1.5792896863938737,0,168.06305601316365,0,0,2,2 +1009,8,1009,1.5777890561648253,0,-666.3866825035068,0,0,2,2 +1010,9,1010,1.5750140106955042,0,-360.3544558297345,0,0,2,2 +1011,10,1011,1.5746689519147377,0,-2898.0569564947527,0,0,2,2 +1012,11,1012,1.5687257363219358,0,-168.259082038207,0,0,2,2 +1013,12,1013,1.5777541674329927,0,110.76121506596262,0,0,2,2 +1014,13,1014,1.5729688000176911,0,-208.97037013342555,0,0,2,2 +1015,14,1015,1.5804808525982976,0,133.11940901234524,0,0,2,2 +1016,15,1016,1.5711309651106926,0,-106.95315866909446,0,0,2,2 +1017,16,1017,1.5737926208556858,0,375.7059874783122,0,0,2,2 +1018,17,1018,1.5761274035452846,0,428.30538553113007,0,0,2,2 +1019,18,1019,1.5702416354892954,0,-169.90136044902954,0,0,2,2 +1020,19,1020,1.578198355029532,0,125.67993567488075,0,0,2,2 +1021,20,1021,1.5720181705458571,0,-161.80746750222914,0,0,2,2 +1022,21,1022,1.5706055651130575,0,-707.9117613317552,0,0,2,2 +1023,22,1023,1.568668187450491,0,-516.1616236843148,0,0,2,2 +1024,23,1024,1.5738116929901291,0,194.4199325330892,0,0,2,2 +1025,24,1025,1.5681610697806125,0,-176.97163001699232,0,0,2,2 +1026,25,1026,1.576233551996162,0,123.87763432587761,0,0,2,2 +1027,26,1027,1.5771205911255413,0,1127.3459838236336,0,0,2,2 +1028,27,1028,1.572915733582594,0,-237.82018529433887,0,0,2,2 +1029,28,1029,1.5762353397531932,0,301.2405534297222,0,0,2,2 +1030,29,1030,1.5753996131209225,0,-1196.5635189618679,0,0,2,2 +1031,30,1031,1.583323885143584,0,126.19455732214087,0,0,2,2 +1032,31,1032,1.5763200772449486,0,-142.77947289143017,0,0,2,2 +1033,32,1033,1.581582891357367,0,190.01241135238598,0,0,2,2 +1034,33,1034,1.587602910539133,0,166.11242751998466,0,0,2,2 +1035,34,1035,1.567492964011488,0,-49.7266364495457,0,0,2,2 +1036,35,1036,1.580190068162111,0,78.75811587722826,0,0,2,2 +1037,36,1037,1.5753588285894533,0,-206.98621646905497,0,0,2,2 +1038,37,1038,1.5770779342184096,0,581.6978219116953,0,0,2,2 +1039,38,1039,1.5778070212815072,0,1371.578307467697,0,0,2,2 +1040,39,1040,1.57688054066635,0,-1079.3533978369474,0,0,2,2 +1041,40,1041,1.5693145277312603,0,-132.17000930069833,0,0,2,2 +1042,41,1042,1.5765282755232228,0,138.62419769016512,0,0,2,2 +1043,42,1043,1.5620408548588791,0,-69.02539956344258,0,0,2,2 +1044,43,1044,1.574392985650641,0,80.95769198517125,0,0,2,2 +1045,44,1045,1.5781552304956135,0,265.7987561166538,0,0,2,2 +1046,45,1046,1.5636488564992264,0,-68.9352142891846,0,0,2,2 +1047,46,1047,1.576282511813594,0,79.15365546365315,0,0,2,2 +1048,47,1048,1.5706338534883697,0,-177.0331895513095,0,0,2,2 +1049,48,1049,1.5697332764760084,0,-1110.3992065908515,0,0,2,2 +1050,49,1050,1.572074578284328,0,427.1128123877896,0,0,2,2 +1051,50,1051,1.5709788389795292,0,-912.6258368395728,0,0,2,2 +1052,51,1052,1.5707963267,0,-5479.083394168603,0,0,2,2 +1053,52,1053,1.5750698586722753,0,233.99848333592325,0,0,2,2 +1054,53,1054,1.5720016133095074,0,-325.9191758699087,0,0,2,2 +1055,54,1055,1.5710102507774575,0,-1008.7127238228784,0,0,2,2 +1056,55,1056,1.5741634461732454,0,317.1386084528101,0,0,2,2 +1057,56,1057,1.5697313502942065,0,-225.6268878860186,0,0,2,2 +1058,57,1058,1.5783787159974525,0,115.64215442219911,0,0,2,2 +1059,58,1059,1.5701001710798361,0,-120.7941745743319,0,0,2,2 +1060,59,1060,1.5760334581575526,0,168.54063976706658,0,0,2,2 +1061,60,1061,1.5701780200451096,0,-170.78141392613784,0,0,2,2 +1062,61,1062,1.5739481064805068,0,265.2459080542718,0,0,2,2 +1063,62,1063,1.5760566970430587,0,474.25043901824483,0,0,2,2 +1064,63,1064,1.5740497522036763,0,-498.2697981413879,0,0,2,2 +1065,64,1065,1.5734512687362876,0,-1670.8899317858372,0,0,2,2 +1066,65,1066,1.5766194230917199,0,315.641186574559,0,0,2,2 +1067,66,1067,1.5694254082809356,0,-139.00443998265538,0,0,2,2 +1068,67,1068,1.5794433316084266,0,99.82108739600862,0,0,2,2 +1069,68,1069,1.5725889467252774,0,-145.8920117629209,0,0,2,2 +1070,69,1070,1.5807873561845662,0,121.97487878176102,0,0,2,2 +1071,70,1071,1.5739130500618594,0,-145.46922731544663,0,0,2,2 +1072,71,1072,1.5718091127823781,0,-475.29933983893517,0,0,2,2 +1073,72,1073,1.5707963267,0,-987.3753375954086,0,0,2,2 +1074,73,1074,1.5787771988715407,0,125.29958862966704,0,0,2,2 +1075,74,1075,1.5676375831346219,0,-89.76970333777409,0,0,2,2 +1076,75,1076,1.5799596823472497,0,81.15500311628622,0,0,2,2 +1077,76,1077,1.5718318856079825,0,-123.03457284663308,0,0,2,2 +1078,77,1078,1.573141750227899,0,763.4376750046916,0,0,2,2 +1079,78,1079,1.5728167843087149,0,-3077.245769373193,0,0,2,2 +1080,79,1080,1.5785502637358915,0,174.41416031947614,0,0,2,2 +1081,80,1081,1.569674339102679,0,-112.66431851597149,0,0,2,2 +1082,81,1082,1.5762914026465802,0,151.12443659721072,0,0,2,2 +1083,82,1083,1.5779954635538054,0,586.8334845075173,0,0,2,2 +1084,83,1084,1.5718615508136478,0,-163.02807724230902,0,0,2,2 +1085,84,1085,1.5801819377192137,0,120.18671864057802,0,0,2,2 +1086,85,1086,1.5735396017275092,0,-150.54944544342285,0,0,2,2 +1087,86,1087,1.5747067556354672,0,856.7850333890341,0,0,2,2 +1088,87,1088,1.5794144265235939,0,212.4192671416631,0,0,2,2 +1089,88,1089,1.5679123444827223,0,-86.94078136867701,0,0,2,2 +1090,89,1090,1.5776885650677432,0,102.28901765291539,0,0,2,2 +1091,90,1091,1.5772802126750645,0,-2448.8652887277844,0,0,2,2 +1092,91,1092,1.5627397146614799,0,-68.77343534353095,0,0,2,2 +1093,92,1093,1.585936009663618,0,43.110332917727945,0,0,2,2 +1094,93,1094,1.5619935809487495,0,-41.7668571517556,0,0,2,2 +1095,94,1095,1.5876906095116257,0,38.91500519420658,0,0,2,2 +1096,95,1096,1.5454624617424062,0,-23.680887105564942,0,0,2,2 +1097,96,1097,1.5873645175601516,0,23.86517750703063,0,0,2,2 +1098,97,1098,1.570944021915078,0,-60.89950155067454,0,0,2,2 +1099,98,1099,1.5807948187074246,0,101.51463085472905,0,0,2,2 +1100,99,1100,1.571359730727884,0,-105.9873529709988,0,0,2,2 +1101,100,1101,1.5698360710217596,0,-656.3145274371365,0,0,2,2 +1102,101,1102,1.5778075823117197,0,125.4467269286155,0,0,2,2 +1103,102,1103,1.5842673129254203,0,154.8052170904881,0,0,2,2 +1104,103,1104,1.577986173407375,0,-159.20678041413706,0,0,2,2 +1105,104,1105,1.5728062349336525,0,-193.0524860619341,0,0,2,2 +1106,105,1106,1.5681396089673467,0,-214.28758319613829,0,0,2,2 +1107,106,1107,1.5668894971276457,0,-799.9284289949252,0,0,2,2 +1108,107,1108,1.576207785892031,0,107.31584149034198,0,0,2,2 +1109,108,1109,1.5583652788716873,0,-56.04593563336265,0,0,2,2 +1110,109,1110,1.577050748676428,0,53.51751978675366,0,0,2,2 +1111,110,1111,1.5845470828584913,0,133.39853529912384,0,0,2,2 +1112,111,1112,1.574603990145371,0,-100.57232984265129,0,0,2,2 +1113,112,1113,1.5714870138239672,0,-320.8237397034875,0,0,2,2 +1114,113,1114,1.5780119468725389,0,153.25827752652526,0,0,2,2 +1115,114,1115,1.5843865999601219,0,156.87128166203587,0,0,2,2 +1116,115,1116,1.5557032253732601,0,-34.863401339744854,0,0,2,2 +1117,116,1117,1.576884531537484,0,47.211441647968,0,0,2,2 +1118,117,1118,1.5766306482410994,0,-3938.817615182425,0,0,2,2 +1119,118,1119,1.5734950747595073,0,-318.9209265452281,0,0,2,2 +1120,119,1120,1.5775238109552248,0,248.21679837538343,0,0,2,2 +1121,120,1121,1.5735982325932878,0,-254.73953333759928,0,0,2,2 +1122,121,1122,1.5701436241046975,0,-289.46840236824585,0,0,2,2 +1123,122,1123,1.5758215401952662,0,176.1209542460578,0,0,2,2 +1124,123,1124,1.558982377070755,0,-59.385374000230954,0,0,2,2 +1125,124,1125,1.5763367431657738,0,57.62238704224561,0,0,2,2 +1126,125,1126,1.564720140088975,0,-86.0836849971439,0,0,2,2 +1127,126,1127,1.576122881314287,0,87.69821047768663,0,0,2,2 +1128,127,1128,1.579527990768055,0,293.6763160119224,0,0,2,2 +1129,128,1129,1.566197129455978,0,-75.01390769807594,0,0,2,2 +1130,129,1130,1.5793387649752906,0,76.09402943267055,0,0,2,2 +1131,130,1131,1.5738126512839616,0,-180.95899864838228,0,0,2,2 +1132,131,1132,1.5740190032649894,0,4846.088683128463,0,0,2,2 +1133,132,1133,1.5713930387454167,0,-380.8124567359731,0,0,2,2 +1134,133,1134,1.578585527369067,0,139.0339355854949,0,0,2,2 +1135,134,1135,1.5764902367863745,0,-477.2607714940234,0,0,2,2 +1136,135,1136,1.573288419766056,0,-312.32265730804284,0,0,2,2 +1137,136,1137,1.579542170941775,0,159.90402750314496,0,0,2,2 +1138,137,1138,1.573978822436062,0,-179.74786209656133,0,0,2,2 +1139,138,1139,1.5810142195432424,0,142.13838746634403,0,0,2,2 +1140,139,1140,1.5716636063774267,0,-106.94485829612124,0,0,2,2 +1141,140,1141,1.5687473345739384,0,-342.9035657114844,0,0,2,2 +1142,141,1142,1.5755972265830727,0,145.98770296911493,0,0,2,2 +1143,142,1143,1.576111637781805,0,1943.9701205268652,0,0,2,2 +1144,143,1144,1.5846480915453838,0,117.14466307620009,0,0,2,2 +1145,144,1145,1.5696541500161478,0,-66.69360408336591,0,0,2,2 +1146,145,1146,1.5665878699312612,0,-326.12806798990545,0,0,2,2 +1147,146,1147,1.5736245608080908,0,142.11225382840146,0,0,2,2 +1148,147,1148,1.5818404302554914,0,121.71566337588133,0,0,2,2 +1149,148,1149,1.5635649606831077,0,-54.71815627167872,0,0,2,2 +1150,149,1150,1.564744372765147,0,847.8800711205324,0,0,2,2 +1151,150,1151,1.573462502288806,0,114.70350346209469,0,0,2,2 +1152,151,1152,1.578323109330659,0,205.73561931449584,0,0,2,2 +1153,152,1153,1.5844978766933193,0,161.94942113076553,0,0,2,2 +1154,153,1154,1.5712725171401973,0,-75.61231102892319,0,0,2,2 +1155,154,1155,1.5788883820139301,0,131.3048506741517,0,0,2,2 +1156,155,1156,1.572986141831219,0,-169.42719527565234,0,0,2,2 +1157,156,1157,1.5740141579748792,0,972.7473699387008,0,0,2,2 +1158,157,1158,1.568363598139526,0,-176.97361485200645,0,0,2,2 +1159,158,1159,1.5809705392166384,0,79.32138287022534,0,0,2,2 diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv new file mode 100644 index 00000000..88652956 --- /dev/null +++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv @@ -0,0 +1,159 @@ +LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,CrossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG +1001,1001,0,1002,1001,1002,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1002,1002,1001,1003,1002,1003,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1003,1003,1002,1004,1003,1004,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1004,1004,1003,1005,1004,1005,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1005,1005,1004,1006,1005,1006,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1006,1006,1005,1007,1006,1007,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1007,1007,1006,1008,1007,1008,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1008,1008,1007,1009,1008,1009,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1009,1009,1008,1010,1009,1010,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1010,1010,1009,1011,1010,1011,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1011,1011,1010,1012,1011,1012,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1012,1012,1011,1013,1012,1013,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1013,1013,1012,1014,1013,1014,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1014,1014,1013,1015,1014,1015,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1015,1015,1014,1016,1015,1016,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1016,1016,1015,1017,1016,1017,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1017,1017,1016,1018,1017,1018,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1018,1018,1017,1019,1018,1019,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1019,1019,1018,1020,1019,1020,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1020,1020,1019,1021,1020,1021,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1021,1021,1020,1022,1021,1022,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1022,1022,1021,1023,1022,1023,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1023,1023,1022,1024,1023,1024,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1024,1024,1023,1025,1024,1025,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1025,1025,1024,1026,1025,1026,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1026,1026,1025,1027,1026,1027,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1027,1027,1026,1028,1027,1028,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1028,1028,1027,1029,1028,1029,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1029,1029,1028,1030,1029,1030,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1030,1030,1029,1031,1030,1031,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1031,1031,1030,1032,1031,1032,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1032,1032,1031,1033,1032,1033,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1033,1033,1032,1034,1033,1034,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1034,1034,1033,1035,1034,1035,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1035,1035,1034,1036,1035,1036,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1036,1036,1035,1037,1036,1037,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1037,1037,1036,1038,1037,1038,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1038,1038,1037,1039,1038,1039,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1039,1039,1038,1040,1039,1040,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1040,1040,1039,1041,1040,1041,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1041,1041,1040,1042,1041,1042,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1042,1042,1041,1043,1042,1043,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1043,1043,1042,1044,1043,1044,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1044,1044,1043,1045,1044,1045,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1045,1045,1044,1046,1045,1046,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1046,1046,1045,1047,1046,1047,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1047,1047,1046,1048,1047,1048,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1048,1048,1047,1049,1048,1049,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1049,1049,1048,1050,1049,1050,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1050,1050,1049,1051,1050,1051,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1051,1051,1050,1052,1051,1052,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1052,1052,1051,1053,1052,1053,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1053,1053,1052,1054,1053,1054,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1054,1054,1053,1055,1054,1055,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1055,1055,1054,1056,1055,1056,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1056,1056,1055,1057,1056,1057,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1057,1057,1056,1058,1057,1058,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1058,1058,1057,1059,1058,1059,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1059,1059,1058,1060,1059,1060,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1060,1060,1059,1061,1060,1061,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1061,1061,1060,1062,1061,1062,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1062,1062,1061,1063,1062,1063,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1063,1063,1062,1064,1063,1064,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1064,1064,1063,1065,1064,1065,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1065,1065,1064,1066,1065,1066,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1066,1066,1065,1067,1066,1067,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1067,1067,1066,1068,1067,1068,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1068,1068,1067,1069,1068,1069,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1069,1069,1068,1070,1069,1070,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1070,1070,1069,1071,1070,1071,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1071,1071,1070,1072,1071,1072,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1072,1072,1071,1073,1072,1073,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1073,1073,1072,1074,1073,1074,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1074,1074,1073,1075,1074,1075,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1075,1075,1074,1076,1075,1076,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1076,1076,1075,1077,1076,1077,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1077,1077,1076,1078,1077,1078,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1078,1078,1077,1079,1078,1079,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1079,1079,1078,1080,1079,1080,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1080,1080,1079,1081,1080,1081,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1081,1081,1080,1082,1081,1082,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1082,1082,1081,1083,1082,1083,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1083,1083,1082,1084,1083,1084,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1084,1084,1083,1085,1084,1085,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1085,1085,1084,1086,1085,1086,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1086,1086,1085,1087,1086,1087,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1087,1087,1086,1088,1087,1088,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1088,1088,1087,1089,1088,1089,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1089,1089,1088,1090,1089,1090,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1090,1090,1089,1091,1090,1091,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1091,1091,1090,1092,1091,1092,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1092,1092,1091,1093,1092,1093,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1093,1093,1092,1094,1093,1094,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1094,1094,1093,1095,1094,1095,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1095,1095,1094,1096,1095,1096,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1096,1096,1095,1097,1096,1097,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1097,1097,1096,1098,1097,1098,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1098,1098,1097,1099,1098,1099,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1099,1099,1098,1100,1099,1100,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1100,1100,1099,1101,1100,1101,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1101,1101,1100,1102,1101,1102,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1102,1102,1101,1103,1102,1103,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1103,1103,1102,1104,1103,1104,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1104,1104,1103,1105,1104,1105,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1105,1105,1104,1106,1105,1106,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1106,1106,1105,1107,1106,1107,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1107,1107,1106,1108,1107,1108,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1108,1108,1107,1109,1108,1109,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1109,1109,1108,1110,1109,1110,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1110,1110,1109,1111,1110,1111,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1111,1111,1110,1112,1111,1112,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1112,1112,1111,1113,1112,1113,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1113,1113,1112,1114,1113,1114,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1114,1114,1113,1115,1114,1115,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1115,1115,1114,1116,1115,1116,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1116,1116,1115,1117,1116,1117,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1117,1117,1116,1118,1117,1118,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1118,1118,1117,1119,1118,1119,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1119,1119,1118,1120,1119,1120,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1120,1120,1119,1121,1120,1121,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1121,1121,1120,1122,1121,1122,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1122,1122,1121,1123,1122,1123,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1123,1123,1122,1124,1123,1124,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1124,1124,1123,1125,1124,1125,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1125,1125,1124,1126,1125,1126,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1126,1126,1125,1127,1126,1127,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1127,1127,1126,1128,1127,1128,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1128,1128,1127,1129,1128,1129,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1129,1129,1128,1130,1129,1130,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1130,1130,1129,1131,1130,1131,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1131,1131,1130,1132,1131,1132,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1132,1132,1131,1133,1132,1133,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1133,1133,1132,1134,1133,1134,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1134,1134,1133,1135,1134,1135,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1135,1135,1134,1136,1135,1136,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1136,1136,1135,1137,1136,1137,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1137,1137,1136,1138,1137,1138,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1138,1138,1137,1139,1138,1139,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1139,1139,1138,1140,1139,1140,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1140,1140,1139,1141,1140,1141,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1141,1141,1140,1142,1141,1142,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1142,1142,1141,1143,1142,1143,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1143,1143,1142,1144,1143,1144,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1144,1144,1143,1145,1144,1145,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1145,1145,1144,1146,1145,1146,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1146,1146,1145,1147,1146,1147,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1147,1147,1146,1148,1147,1148,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1148,1148,1147,1149,1148,1149,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1149,1149,1148,1150,1149,1150,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1150,1150,1149,1151,1150,1151,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1151,1151,1150,1152,1151,1152,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1152,1152,1151,1153,1152,1153,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1153,1153,1152,1154,1153,1154,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1154,1154,1153,1155,1154,1155,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1155,1155,1154,1156,1155,1156,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1156,1156,1155,1157,1156,1157,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1157,1157,1156,1158,1157,1158,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +1158,1158,1157,0,1158,1159,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv new file mode 100644 index 00000000..de7375dd --- /dev/null +++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv @@ -0,0 +1,160 @@ +NID,PID +1001,1001 +1002,1002 +1003,1003 +1004,1004 +1005,1005 +1006,1006 +1007,1007 +1008,1008 +1009,1009 +1010,1010 +1011,1011 +1012,1012 +1013,1013 +1014,1014 +1015,1015 +1016,1016 +1017,1017 +1018,1018 +1019,1019 +1020,1020 +1021,1021 +1022,1022 +1023,1023 +1024,1024 +1025,1025 +1026,1026 +1027,1027 +1028,1028 +1029,1029 +1030,1030 +1031,1031 +1032,1032 +1033,1033 +1034,1034 +1035,1035 +1036,1036 +1037,1037 +1038,1038 +1039,1039 +1040,1040 +1041,1041 +1042,1042 +1043,1043 +1044,1044 +1045,1045 +1046,1046 +1047,1047 +1048,1048 +1049,1049 +1050,1050 +1051,1051 +1052,1052 +1053,1053 +1054,1054 +1055,1055 +1056,1056 +1057,1057 +1058,1058 +1059,1059 +1060,1060 +1061,1061 +1062,1062 +1063,1063 +1064,1064 +1065,1065 +1066,1066 +1067,1067 +1068,1068 +1069,1069 +1070,1070 +1071,1071 +1072,1072 +1073,1073 +1074,1074 +1075,1075 +1076,1076 +1077,1077 +1078,1078 +1079,1079 +1080,1080 +1081,1081 +1082,1082 +1083,1083 +1084,1084 +1085,1085 +1086,1086 +1087,1087 +1088,1088 +1089,1089 +1090,1090 +1091,1091 +1092,1092 +1093,1093 +1094,1094 +1095,1095 +1096,1096 +1097,1097 +1098,1098 +1099,1099 +1100,1100 +1101,1101 +1102,1102 +1103,1103 +1104,1104 +1105,1105 +1106,1106 +1107,1107 +1108,1108 +1109,1109 +1110,1110 +1111,1111 +1112,1112 +1113,1113 +1114,1114 +1115,1115 +1116,1116 +1117,1117 +1118,1118 +1119,1119 +1120,1120 +1121,1121 +1122,1122 +1123,1123 +1124,1124 +1125,1125 +1126,1126 +1127,1127 +1128,1128 +1129,1129 +1130,1130 +1131,1131 +1132,1132 +1133,1133 +1134,1134 +1135,1135 +1136,1136 +1137,1137 +1138,1138 +1139,1139 +1140,1140 +1141,1141 +1142,1142 +1143,1143 +1144,1144 +1145,1145 +1146,1146 +1147,1147 +1148,1148 +1149,1149 +1150,1150 +1151,1151 +1152,1152 +1153,1153 +1154,1154 +1155,1155 +1156,1156 +1157,1157 +1158,1158 +1159,1159 diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv new file mode 100644 index 00000000..0f778cae --- /dev/null +++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv @@ -0,0 +1,160 @@ +PID,B(Lat),L(Long),H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3 +1001,0,0,2.458,127.057,258.58,0,0,0,0 +1002,0,0,2.459,128.123,258.573,0,0,0,0 +1003,0,0,2.459,129.339,258.569,0,0,0,0 +1004,0,0,2.459,130.567,258.567,0,0,0,0 +1005,0,0,2.46,131.7,258.559,0,0,0,0 +1006,0,0,2.462,132.796,258.564,0,0,0,0 +1007,0,0,2.462,134.128,258.56,0,0,0,0 +1008,0,0,2.461,135.17,258.551,0,0,0,0 +1009,0,0,2.463,136.475,258.542,0,0,0,0 +1010,0,0,2.463,137.611,258.537,0,0,0,0 +1011,0,0,2.463,138.785,258.533,0,0,0,0 +1012,0,0,2.464,139.891,258.535,0,0,0,0 +1013,0,0,2.462,141.114,258.527,0,0,0,0 +1014,0,0,2.461,142.477,258.524,0,0,0,0 +1015,0,0,2.461,143.766,258.511,0,0,0,0 +1016,0,0,2.461,145.134,258.511,0,0,0,0 +1017,0,0,2.461,146.549,258.506,0,0,0,0 +1018,0,0,2.461,147.877,258.499,0,0,0,0 +1019,0,0,2.46,149.143,258.5,0,0,0,0 +1020,0,0,2.459,150.371,258.491,0,0,0,0 +1021,0,0,2.457,151.52,258.49,0,0,0,0 +1022,0,0,2.454,152.64,258.49,0,0,0,0 +1023,0,0,2.457,153.716,258.492,0,0,0,0 +1024,0,0,2.46,154.738,258.489,0,0,0,0 +1025,0,0,2.457,156.035,258.492,0,0,0,0 +1026,0,0,2.455,157.264,258.486,0,0,0,0 +1027,0,0,2.455,158.48,258.478,0,0,0,0 +1028,0,0,2.454,159.704,258.475,0,0,0,0 +1029,0,0,2.455,160.865,258.469,0,0,0,0 +1030,0,0,2.456,161.939,258.464,0,0,0,0 +1031,0,0,2.48,163.143,258.449,0,0,0,0 +1032,0,0,2.51,164.181,258.443,0,0,0,0 +1033,0,0,2.518,165.358,258.431,0,0,0,0 +1034,0,0,2.548,166.424,258.413,0,0,0,0 +1035,0,0,2.557,167.449,258.416,0,0,0,0 +1036,0,0,2.556,168.57,258.406,0,0,0,0 +1037,0,0,2.557,169.594,258.401,0,0,0,0 +1038,0,0,2.556,170.677,258.394,0,0,0,0 +1039,0,0,2.56,171.822,258.386,0,0,0,0 +1040,0,0,2.557,173.021,258.379,0,0,0,0 +1041,0,0,2.559,174.297,258.381,0,0,0,0 +1042,0,0,2.543,175.655,258.373,0,0,0,0 +1043,0,0,2.508,176.851,258.383,0,0,0,0 +1044,0,0,2.501,177.954,258.379,0,0,0,0 +1045,0,0,2.47,179.003,258.372,0,0,0,0 +1046,0,0,2.452,180.062,258.379,0,0,0,0 +1047,0,0,2.454,181.096,258.374,0,0,0,0 +1048,0,0,2.455,182.411,258.374,0,0,0,0 +1049,0,0,2.455,183.703,258.375,0,0,0,0 +1050,0,0,2.454,184.753,258.374,0,0,0,0 +1051,0,0,2.451,185.757,258.374,0,0,0,0 +1052,0,0,2.455,187.03,258.374,0,0,0,0 +1053,0,0,2.454,188.244,258.368,0,0,0,0 +1054,0,0,2.456,189.383,258.367,0,0,0,0 +1055,0,0,2.457,190.667,258.367,0,0,0,0 +1056,0,0,2.458,191.863,258.363,0,0,0,0 +1057,0,0,2.459,193.009,258.364,0,0,0,0 +1058,0,0,2.46,194.084,258.356,0,0,0,0 +1059,0,0,2.458,195.18,258.357,0,0,0,0 +1060,0,0,2.458,196.497,258.35,0,0,0,0 +1061,0,0,2.459,197.731,258.351,0,0,0,0 +1062,0,0,2.459,198.941,258.347,0,0,0,0 +1063,0,0,2.462,200.026,258.341,0,0,0,0 +1064,0,0,2.458,201.086,258.338,0,0,0,0 +1065,0,0,2.459,202.086,258.335,0,0,0,0 +1066,0,0,2.458,203.396,258.327,0,0,0,0 +1067,0,0,2.455,204.465,258.329,0,0,0,0 +1068,0,0,2.456,205.481,258.32,0,0,0,0 +1069,0,0,2.458,206.775,258.318,0,0,0,0 +1070,0,0,2.458,207.835,258.307,0,0,0,0 +1071,0,0,2.457,209.0,258.303,0,0,0,0 +1072,0,0,2.457,210.085,258.302,0,0,0,0 +1073,0,0,2.46,211.148,258.302,0,0,0,0 +1074,0,0,2.457,212.268,258.293,0,0,0,0 +1075,0,0,2.456,213.408,258.297,0,0,0,0 +1076,0,0,2.457,214.507,258.287,0,0,0,0 +1077,0,0,2.455,215.509,258.286,0,0,0,0 +1078,0,0,2.458,216.589,258.283,0,0,0,0 +1079,0,0,2.458,217.782,258.281,0,0,0,0 +1080,0,0,2.457,218.88,258.272,0,0,0,0 +1081,0,0,2.456,219.968,258.274,0,0,0,0 +1082,0,0,2.457,221.04,258.268,0,0,0,0 +1083,0,0,2.457,222.66,258.256,0,0,0,0 +1084,0,0,2.457,223.777,258.255,0,0,0,0 +1085,0,0,2.456,224.837,258.245,0,0,0,0 +1086,0,0,2.458,225.871,258.242,0,0,0,0 +1087,0,0,2.456,227.518,258.236,0,0,0,0 +1088,0,0,2.455,228.57,258.227,0,0,0,0 +1089,0,0,2.457,229.861,258.23,0,0,0,0 +1090,0,0,2.455,231.087,258.222,0,0,0,0 +1091,0,0,2.455,232.231,258.214,0,0,0,0 +1092,0,0,2.455,233.329,258.223,0,0,0,0 +1093,0,0,2.454,234.347,258.208,0,0,0,0 +1094,0,0,2.453,235.55,258.218,0,0,0,0 +1095,0,0,2.453,236.672,258.2,0,0,0,0 +1096,0,0,2.454,237.704,258.226,0,0,0,0 +1097,0,0,2.452,238.879,258.206,0,0,0,0 +1098,0,0,2.451,239.912,258.206,0,0,0,0 +1099,0,0,2.448,241.081,258.194,0,0,0,0 +1100,0,0,2.446,242.165,258.194,0,0,0,0 +1101,0,0,2.438,243.309,258.195,0,0,0,0 +1102,0,0,2.443,244.471,258.187,0,0,0,0 +1103,0,0,2.443,245.583,258.172,0,0,0,0 +1104,0,0,2.442,246.585,258.165,0,0,0,0 +1105,0,0,2.442,247.906,258.162,0,0,0,0 +1106,0,0,2.442,248.917,258.165,0,0,0,0 +1107,0,0,2.442,250.01,258.169,0,0,0,0 +1108,0,0,2.44,251.104,258.163,0,0,0,0 +1109,0,0,2.44,252.201,258.177,0,0,0,0 +1110,0,0,2.442,253.26,258.17,0,0,0,0 +1111,0,0,2.443,254.412,258.154,0,0,0,0 +1112,0,0,2.446,255.422,258.15,0,0,0,0 +1113,0,0,2.448,256.615,258.149,0,0,0,0 +1114,0,0,2.45,257.748,258.141,0,0,0,0 +1115,0,0,2.45,259.226,258.121,0,0,0,0 +1116,0,0,2.451,260.477,258.14,0,0,0,0 +1117,0,0,2.449,261.941,258.131,0,0,0,0 +1118,0,0,2.451,263.243,258.124,0,0,0,0 +1119,0,0,2.45,264.476,258.12,0,0,0,0 +1120,0,0,2.449,265.628,258.112,0,0,0,0 +1121,0,0,2.453,266.706,258.109,0,0,0,0 +1122,0,0,2.451,267.735,258.11,0,0,0,0 +1123,0,0,2.449,268.913,258.104,0,0,0,0 +1124,0,0,2.45,270.088,258.118,0,0,0,0 +1125,0,0,2.45,271.19,258.112,0,0,0,0 +1126,0,0,2.451,272.23,258.118,0,0,0,0 +1127,0,0,2.451,273.427,258.112,0,0,0,0 +1128,0,0,2.452,274.577,258.102,0,0,0,0 +1129,0,0,2.449,275.678,258.107,0,0,0,0 +1130,0,0,2.45,276.814,258.097,0,0,0,0 +1131,0,0,2.449,277.887,258.094,0,0,0,0 +1132,0,0,2.445,279.118,258.09,0,0,0,0 +1133,0,0,2.448,280.141,258.089,0,0,0,0 +1134,0,0,2.449,281.253,258.081,0,0,0,0 +1135,0,0,2.449,282.266,258.075,0,0,0,0 +1136,0,0,2.447,283.271,258.072,0,0,0,0 +1137,0,0,2.448,284.813,258.059,0,0,0,0 +1138,0,0,2.45,286.002,258.055,0,0,0,0 +1139,0,0,2.451,287.101,258.044,0,0,0,0 +1140,0,0,2.45,288.262,258.043,0,0,0,0 +1141,0,0,2.452,289.275,258.045,0,0,0,0 +1142,0,0,2.451,290.578,258.039,0,0,0,0 +1143,0,0,2.452,291.778,258.032,0,0,0,0 +1144,0,0,2.451,292.913,258.017,0,0,0,0 +1145,0,0,2.451,294.195,258.018,0,0,0,0 +1146,0,0,2.453,295.515,258.024,0,0,0,0 +1147,0,0,2.449,296.713,258.02,0,0,0,0 +1148,0,0,2.453,297.854,258.008,0,0,0,0 +1149,0,0,2.454,298.938,258.016,0,0,0,0 +1150,0,0,2.453,300.189,258.023,0,0,0,0 +1151,0,0,2.452,301.196,258.02,0,0,0,0 +1152,0,0,2.451,302.721,258.009,0,0,0,0 +1153,0,0,2.45,304.159,257.989,0,0,0,0 +1154,0,0,2.444,305.249,257.989,0,0,0,0 +1155,0,0,2.444,306.501,257.979,0,0,0,0 +1156,0,0,2.446,307.685,257.976,0,0,0,0 +1157,0,0,2.444,308.786,257.972,0,0,0,0 +1158,0,0,2.44,309.827,257.975,0,0,0,0 +1159,0,0,2.442,311.051,257.963,0,0,0,0 diff --git a/autoware.ai/src/autoware/common/map_file/launch/lanelet2_map_loader.launch b/autoware.ai/src/autoware/common/map_file/launch/lanelet2_map_loader.launch index 75dcd94a..b090cc09 100644 --- a/autoware.ai/src/autoware/common/map_file/launch/lanelet2_map_loader.launch +++ b/autoware.ai/src/autoware/common/map_file/launch/lanelet2_map_loader.launch @@ -1,7 +1,7 @@ - + - + diff --git a/autoware.ai/src/autoware/common/map_file/launch/map_filter.launch b/autoware.ai/src/autoware/common/map_file/launch/map_filter.launch index f77a9ef4..e13c244c 100644 --- a/autoware.ai/src/autoware/common/map_file/launch/map_filter.launch +++ b/autoware.ai/src/autoware/common/map_file/launch/map_filter.launch @@ -1,6 +1,6 @@ - + diff --git a/autoware.ai/src/autoware/common/map_file/launch/vector_map_loader.launch b/autoware.ai/src/autoware/common/map_file/launch/vector_map_loader.launch index 727e16e4..2f6aeb85 100644 --- a/autoware.ai/src/autoware/common/map_file/launch/vector_map_loader.launch +++ b/autoware.ai/src/autoware/common/map_file/launch/vector_map_loader.launch @@ -1,6 +1,6 @@ - + diff --git a/autoware.ai/src/autoware/common/object_map/launch/grid_map_filter.launch b/autoware.ai/src/autoware/common/object_map/launch/grid_map_filter.launch index 9eb23ab3..a6d86fdf 100755 --- a/autoware.ai/src/autoware/common/object_map/launch/grid_map_filter.launch +++ b/autoware.ai/src/autoware/common/object_map/launch/grid_map_filter.launch @@ -10,7 +10,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/autoware.ai/src/autoware/common/object_map/launch/laserscan2costmap.launch b/autoware.ai/src/autoware/common/object_map/launch/laserscan2costmap.launch index ee0a8583..77d3b330 100644 --- a/autoware.ai/src/autoware/common/object_map/launch/laserscan2costmap.launch +++ b/autoware.ai/src/autoware/common/object_map/launch/laserscan2costmap.launch @@ -8,7 +8,7 @@ - + diff --git a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid.launch b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid.launch index f8f6f8cf..8bd5c0c2 100755 --- a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid.launch +++ b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid.launch @@ -10,7 +10,7 @@ - + diff --git a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_lanelet2.launch b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_lanelet2.launch index 6a99445d..f8cec208 100755 --- a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_lanelet2.launch +++ b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_lanelet2.launch @@ -10,7 +10,7 @@ - + diff --git a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_option.launch b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_option.launch index eefa5062..6e9afb05 100644 --- a/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_option.launch +++ b/autoware.ai/src/autoware/common/object_map/launch/wayarea2grid_option.launch @@ -13,7 +13,7 @@ - + @@ -26,7 +26,7 @@ - + diff --git a/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h b/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h index 323aeea3..2cd7f757 100755 --- a/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h +++ b/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h @@ -908,6 +908,14 @@ class PlanningParams double smoothingSmoothWeight; double smoothingToleranceError; + // Added by HYP + int enableDebug; + + // Added by HYP for blocking the trajectories + double lateralBlockingThreshold; + double frontLongitudinalBlockingThreshold; + double rearLongitudinalBlockingThreshold; + // Added by HJW for make traj eval parameter double weightPriority; double weightTransition; @@ -975,11 +983,17 @@ class PlanningParams smoothingSmoothWeight = 0.2; smoothingToleranceError = 0.05; - double weightPriority = 1; - double weightTransition = 1; - double weightLong = 1.2; - double weightLat = 1; - double LateralSkipDistance = 5; + enableDebug = 0; + + lateralBlockingThreshold = 1.5; + frontLongitudinalBlockingThreshold = 30.0; + rearLongitudinalBlockingThreshold = -5.0; + + weightPriority = 1; + weightTransition = 1; + weightLong = 1.2; + weightLat = 1; + LateralSkipDistance = 5; stopSignStopTime = 2.0; diff --git a/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h b/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h index 05d60262..84d5b056 100755 --- a/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h +++ b/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h @@ -58,7 +58,7 @@ class TrajectoryDynamicCosts private: bool ValidateRollOutsInput(const vector > >& rollOuts); vector CalculatePriorityAndLaneChangeCosts(const vector >& laneRollOuts, const int& lane_index, const PlanningParams& params); - void NormalizeCosts(vector& trajectoryCosts); + void NormalizeCosts(vector& trajectoryCosts, int enableDebug); void CalculateLateralAndLongitudinalCosts(vector& trajectoryCosts, const vector > >& rollOuts, const vector >& totalPaths, const WayPoint& currState, const vector& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState); void CalculateLateralAndLongitudinalCostsStatic(vector& trajectoryCosts, const vector >& rollOuts, const vector& totalPaths, const WayPoint& currState, const vector& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState); void CalculateTransitionCosts(vector& trajectoryCosts, const int& currTrajectoryIndex, const PlanningParams& params); diff --git a/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp b/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp index 9edfca2f..d1857f97 100755 --- a/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp +++ b/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp @@ -667,7 +667,7 @@ bool DecisionMaker::SelectSafeTrajectory() desiredVelocity = m_params.maxSpeed; } - if(desiredVelocity < m_params.maxSpeed * m_params.curveVelocityRatio){ // minimum of target velocity is max_speed / 2 + if(desiredVelocity < m_params.maxSpeed * m_params.curveVelocityRatio){ // minimum of target velocity is max_speed * curveVelocityRatio desiredVelocity = m_params.maxSpeed * m_params.curveVelocityRatio; } previous_velocity = desiredVelocity; @@ -739,6 +739,7 @@ bool DecisionMaker::SelectSafeTrajectory() const TrajectoryCost& tc, const bool& bEmergencyStop) { + static double prev_max_velocity = 0.0; PlannerHNS::BehaviorState beh; state = currPose; @@ -766,7 +767,7 @@ bool DecisionMaker::SelectSafeTrajectory() beh.bNewPlan = SelectSafeTrajectory(); beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt); - + //std::cout << "Eval_i: " << tc.index << ", Curr_i: " << m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << ", Prev_i: " << m_pCurrentBehaviorState->GetCalcParams()->iPrevSafeTrajectory << std::endl; return beh; diff --git a/autoware.ai/src/autoware/common/op_planner/src/PlannerH.cpp b/autoware.ai/src/autoware/common/op_planner/src/PlannerH.cpp index 3985a922..8db6358f 100755 --- a/autoware.ai/src/autoware/common/op_planner/src/PlannerH.cpp +++ b/autoware.ai/src/autoware/common/op_planner/src/PlannerH.cpp @@ -147,13 +147,13 @@ double PlannerH::PlanUsingDP(const WayPoint& start, { GPSPoint sp = start.pos; GPSPoint gp = goalPos.pos; - cout << endl << "Error: PlannerH -> Can't Find Global Waypoint Nodes in the Map for Start (" << sp.ToString() << ") and Goal (" << gp.ToString() << ")" << endl; + // cout << endl << "Error: PlannerH -> Can't Find Global Waypoint Nodes in the Map for Start (" << sp.ToString() << ") and Goal (" << gp.ToString() << ")" << endl; return 0; } if(!pStart->pLane || !pGoal->pLane) { - cout << endl << "Error: PlannerH -> Null Lane, Start (" << pStart->pLane << ") and Goal (" << pGoal->pLane << ")" << endl; + // cout << endl << "Error: PlannerH -> Null Lane, Start (" << pStart->pLane << ") and Goal (" << pGoal->pLane << ")" << endl; return 0; } diff --git a/autoware.ai/src/autoware/common/op_planner/src/TrajectoryCosts.cpp b/autoware.ai/src/autoware/common/op_planner/src/TrajectoryCosts.cpp index d4a1ca2d..34cd18e0 100755 --- a/autoware.ai/src/autoware/common/op_planner/src/TrajectoryCosts.cpp +++ b/autoware.ai/src/autoware/common/op_planner/src/TrajectoryCosts.cpp @@ -31,6 +31,7 @@ TrajectoryCost TrajectoryCosts::DoOneStep(const vector > const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState, const std::vector& obj_list) { + /* TrajectoryCost bestTrajectory; bestTrajectory.bBlocked = true; bestTrajectory.closest_obj_distance = params.horizonDistance; @@ -53,6 +54,11 @@ TrajectoryCost TrajectoryCosts::DoOneStep(const vector > } } + std::cout<<"## Initail trajecotry cost"<ToString()< > m_PrevCostIndex = smallestIndex; return bestTrajectory; + */ } void TrajectoryCosts::CalculateLateralAndLongitudinalCosts(vector& trajectoryCosts, @@ -303,6 +310,8 @@ void TrajectoryCosts::NormalizeCosts(vector& trajectoryCosts) trajectoryCosts.at(ic).longitudinal_cost = trajectoryCosts.at(ic).longitudinal_cost / totalLongitudinalCosts; else trajectoryCosts.at(ic).longitudinal_cost = 0; + + std::cout<<"Costs("<& trajectoryCosts) trajectoryCosts.at(ic).lateral_cost = m_WeightLat*trajectoryCosts.at(ic).lateral_cost; trajectoryCosts.at(ic).longitudinal_cost = m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost; + std::cout<<"Weighted Costs("<& obj_list, const int& iCurrentIndex) { + /* TrajectoryCost bestTrajectory; bestTrajectory.bBlocked = true; bestTrajectory.closest_obj_distance = params.horizonDistance; @@ -113,6 +114,7 @@ TrajectoryCost TrajectoryDynamicCosts::DoOneStepDynamic(const vector >& rollOuts, @@ -121,7 +123,7 @@ TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector& obj_list, const PlannerHNS::STATE_TYPE& state, const int& iCurrentIndex) { TrajectoryCost bestTrajectory; - bestTrajectory.bBlocked = true; + bestTrajectory.bBlocked = false; bestTrajectory.closest_obj_distance = params.horizonDistance; bestTrajectory.closest_obj_velocity = 0; bestTrajectory.index = -1; @@ -136,79 +138,56 @@ TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector car_info.perp_point.RightLnId){ - m_endTrajIdx = min(car_info.perp_point.LeftLnId, int(rollOuts.size())-1); - m_startTrajIdx = min(car_info.perp_point.RightLnId, int(rollOuts.size())-1); - } - else{ - m_startTrajIdx = min(car_info.perp_point.LeftLnId, int(rollOuts.size())-1); - m_endTrajIdx = min(car_info.perp_point.RightLnId, int(rollOuts.size())-1); - } + // M + // if(car_info.perp_point.LeftLnId == 100){ + // m_startTrajIdx = rollOuts.size() / 2; + // m_endTrajIdx = rollOuts.size() / 2; + // } + // else if(car_info.perp_point.LeftLnId > car_info.perp_point.RightLnId){ + // m_endTrajIdx = min(car_info.perp_point.LeftLnId, int(rollOuts.size())-1); + // m_startTrajIdx = min(car_info.perp_point.RightLnId, int(rollOuts.size())-1); + // } + // else{ + // m_startTrajIdx = min(car_info.perp_point.LeftLnId, int(rollOuts.size())-1); + // m_endTrajIdx = min(car_info.perp_point.RightLnId, int(rollOuts.size())-1); + // } double minDistanceToRollOut = 0; int currIndex = 0; - - for(int i=0; i direct_distance){ - minDistanceToRollOut = direct_distance; - currIndex = i; - } - } - - //std::cout << "Current Index: " << currIndex << std::endl; + currIndex = GetCurrentRollOutIndex(totalPaths, currState, params); // Calculate lane change cost: Scoring the cost by the distance between current path and candidate path - m_TrajectoryCosts.clear(); - if(rollOuts.size()>0) + int centralIndex = params.rollOutNumber/2; + if(rollOuts.size() % 2 == 0) centralIndex--; + + m_TrajectoryCosts.clear(); + for(unsigned int it=0; it< rollOuts.size(); it++) { TrajectoryCost tc; - int centralIndex = params.rollOutNumber/2; - tc.lane_index = 0; - for(unsigned int it=0; it< rollOuts.size(); it++) - { - tc.index = it; - tc.relative_index = it - centralIndex; // index based on center (central index is 0) - tc.distance_from_center = params.rollOutDensity*tc.relative_index; - tc.priority_cost = fabs(tc.distance_from_center); // priority_cost = distance from center - tc.closest_obj_distance = params.horizonDistance; - if(rollOuts.at(it).size() > 0) - tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost; - m_TrajectoryCosts.push_back(tc); - } + tc.lane_index = it; + tc.index = it; + tc.relative_index = it - centralIndex; // index based on center (central index is 0) + tc.distance_from_center = params.rollOutDensity*tc.relative_index; + tc.priority_cost = fabs(tc.distance_from_center); // priority_cost = distance from center + tc.closest_obj_distance = params.horizonDistance; + if(rollOuts.at(it).size() > 0) + tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost; + m_TrajectoryCosts.push_back(tc); } - // std::cout << m_PrevSelectedIndex << std::endl; if(m_PrevSelectedIndex == -1){ - CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params); + CalculateTransitionCosts(m_TrajectoryCosts, centralIndex, params); } else{ CalculateTransitionCosts(m_TrajectoryCosts, m_PrevSelectedIndex, params); } - //end // Calculate trajectory cost with object WayPoint p; m_AllContourPoints.clear(); for(unsigned int io=0; io m_endTrajIdx; ic--) m_TrajectoryCosts.at(ic).bBlocked = true; + // M + // for(int ic = 0; ic < m_startTrajIdx; ic++) m_TrajectoryCosts.at(ic).bBlocked = true; + // for(int ic = rollOuts.size() - 1; ic > m_endTrajIdx; ic--) m_TrajectoryCosts.at(ic).bBlocked = true; // for(unsigned int ic = std::max(currIndex - 1, m_startTrajIdx); ic <= std::min(currIndex + 1, m_endTrajIdx); ic++) - for(unsigned int ic = m_startTrajIdx; ic <= m_endTrajIdx; ic++) + // for(unsigned int ic = m_startTrajIdx; ic <= m_endTrajIdx; ic++) + + bool is_current_path_blocked = false; + + for(unsigned int ic = 0; ic < rollOuts.size(); ic++) { if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost) - { - #ifdef DEBUG_ENABLE - std::cout << "smallestIndex is Updated" << std::endl; - #endif + { + if(params.enableDebug) std::cout << "smallestIndex is Updated" << std::endl; smallestCost = m_TrajectoryCosts.at(ic).cost; smallestIndex = ic; } @@ -248,104 +229,60 @@ TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector 0){ bAllFree = false; } } - // Enable when needed - // if(bAllFree && smallestIndex >= 0){ - // smallestIndex = params.rollOutNumber/2; - // } - - ////// Change Lane when turn left / right - // // Calculate Turn Angle - // double turn_angle = 0; - - // if(m_PrevSelectedIndex != -1) - // turn_angle = CalculateTurnAngle(rollOuts.at(m_PrevSelectedIndex), currState, 50); - - // // std::cout << "b_all_free : " << bAllFree << " , t_a : " << turn_angle << std::endl; - - // // Keep state when Intersection state - // if(state == PlannerHNS::INTERSECTION_STATE){ - // smallestIndex = m_PrevSelectedIndex; - // } - // // For Left Turn - // else if(bAllFree && turn_angle > 45){ - // smallestIndex = 0; - // } - // // For Right Turn - // else if(bAllFree && turn_angle < -45 && params.rollOutNumber > 0){ - // smallestIndex = params.rollOutNumber - 1; - // } - - // if(smallestIndex >= 0 && m_TrajectoryCosts.at(currIndex).bBlocked){ - // int left_block_idx = -1; - - // for(int ic = currIndex - 1; ic >= m_startTrajIdx; ic--) - // { - // if(m_TrajectoryCosts.at(ic).bBlocked){ - // left_block_idx = ic; - // break; - // } - // } - - // if(left_block_idx != -1 && smallestIndex < left_block_idx){ - // smallestIndex = -1; - // } - - // int right_block_idx = -1; - - // for(int ic = currIndex + 1; ic <= m_endTrajIdx; ic++) - // { - // if(m_TrajectoryCosts.at(ic).bBlocked){ - // right_block_idx = ic; - // break; - // } - // } - - // if(right_block_idx != -1 && smallestIndex > right_block_idx){ - // smallestIndex = -1; - // } - // } - // Hyundai challenge legacy + static int path_keeping_cnt = 20, cur_path_blocking_cnt = 20; + if(m_PrevIndex == currIndex){ + path_keeping_cnt--; + if(path_keeping_cnt < 0) path_keeping_cnt = 0; + } + else path_keeping_cnt = 20; - if(smallestIndex == -1) - { - bestTrajectory.bBlocked = true; - bestTrajectory.lane_index = m_PrevSelectedIndex; - bestTrajectory.index = m_PrevSelectedIndex; - bestTrajectory.closest_obj_distance = smallestDistance; - bestTrajectory.closest_obj_velocity = velo_of_next; + // Change lane if current path is blocked or current path is not center and the ego keeps lane 1 sceonds(When rate is 20) + if((is_current_path_blocked) || (smallestIndex != params.rollOutNumber/2 && path_keeping_cnt == 0)){ + if(smallestIndex == -1) + { + bestTrajectory.bBlocked = true; + bestTrajectory.lane_index = m_PrevSelectedIndex; + bestTrajectory.index = m_PrevSelectedIndex; + bestTrajectory.closest_obj_distance = smallestDistance; + bestTrajectory.closest_obj_velocity = velo_of_next; + } + else if(smallestIndex >= 0) + { + bestTrajectory = m_TrajectoryCosts.at(smallestIndex); + m_PrevSelectedIndex = smallestIndex; + } } - else if(smallestIndex >= 0) - { - bestTrajectory = m_TrajectoryCosts.at(smallestIndex); - m_PrevSelectedIndex = smallestIndex; + + m_PrevIndex = currIndex; + + if(params.enableDebug){ + for(unsigned int ic=0; ic& obj_list) { + /* TrajectoryCost bestTrajectory; bestTrajectory.bBlocked = true; bestTrajectory.closest_obj_distance = params.horizonDistance; @@ -397,17 +335,15 @@ TrajectoryCost TrajectoryDynamicCosts::DoOneStep(const vector= 0) { bestTrajectory = m_TrajectoryCosts.at(smallestIndex); - //bestTrajectory.index = smallestIndex; } -// cout << "smallestI: " << smallestIndex << ", C_Size: " << m_TrajectoryCosts.size() -// << ", LaneI: " << bestTrajectory.lane_index << "TrajI: " << bestTrajectory.index -// << ", prevSmalI: " << m_PrevCostIndex << ", distance: " << bestTrajectory.closest_obj_distance -// << ", Blocked: " << bestTrajectory.bBlocked -// << endl; - m_PrevCostIndex = smallestIndex; return bestTrajectory; + */ } void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector& trajectoryCosts, @@ -501,9 +428,8 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector 0 && rollOuts.at(0).size()>0) @@ -513,9 +439,7 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector params.minFollowingDistance || lateralDist > m_LateralSkipDistance) @@ -568,14 +491,15 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector= -5 - && longitudinalDist < 30) + if( lateralDist <= params.lateralBlockingThreshold + && longitudinalDist < params.frontLongitudinalBlockingThreshold + && longitudinalDist >= params.rearLongitudinalBlockingThreshold){ trajectoryCosts.at(it).bBlocked = true; + } // Original // if(lateralDist <= critical_lateral_distance @@ -596,17 +520,15 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector= -carInfo.length/1.5 - && longitudinalDist < params.minFollowingDistance) + if(lateralDist <= critical_lateral_distance && longitudinalDist >= -carInfo.length/1.5 && longitudinalDist < params.minFollowingDistance){ trajectoryCosts.at(iCostIndex).bBlocked = true; + } if(lateralDist != 0) @@ -739,7 +661,7 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCosts(vector& trajectoryCosts) +void TrajectoryDynamicCosts::NormalizeCosts(vector& trajectoryCosts, int enableDebug) { //Normalize costs double totalPriorities = 0; @@ -791,22 +713,19 @@ void TrajectoryDynamicCosts::NormalizeCosts(vector& trajectoryCo trajectoryCosts.at(ic).cost = (m_WeightPriority*trajectoryCosts.at(ic).priority_cost + m_WeightTransition*trajectoryCosts.at(ic).transition_cost + m_WeightLat*trajectoryCosts.at(ic).lateral_cost + m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost)/4.0; - #ifdef DEBUG_ENABLE - std::cout << "Index: " << ic - << ", Priority: " << trajectoryCosts.at(ic).priority_cost - << ", Transition: " << trajectoryCosts.at(ic).transition_cost - << ", Lat: " << trajectoryCosts.at(ic).lateral_cost - << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost - << ", Change: " << trajectoryCosts.at(ic).lane_change_cost - << ", Avg: " << trajectoryCosts.at(ic).cost - << ", Blocked : " << trajectoryCosts.at(ic).bBlocked - << std::endl; - #endif + if(enableDebug){ + std::cout << "Index: " << ic + << ", Priority: " << trajectoryCosts.at(ic).priority_cost + << ", Transition: " << trajectoryCosts.at(ic).transition_cost + << ", Lat: " << trajectoryCosts.at(ic).lateral_cost + << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost + << ", Change: " << trajectoryCosts.at(ic).lane_change_cost + << ", Avg: " << trajectoryCosts.at(ic).cost + << ", Blocked : " << trajectoryCosts.at(ic).bBlocked + << std::endl; + } } - - #ifdef DEBUG_ENABLE - std::cout << "------------------------ " << std::endl; - #endif + if(enableDebug) std::cout << "------------------------ " << std::endl; } vector TrajectoryDynamicCosts::CalculatePriorityAndLaneChangeCosts(const vector >& laneRollOuts, @@ -905,9 +824,13 @@ void TrajectoryDynamicCosts::CalculateIntersectionVelocities(const std::vector

& path, const WayPoint& currState, const PlanningParams& params) { - RelativeInfo obj_info; - PlanningHelpers::GetRelativeInfo(path, currState, obj_info); - int currIndex = params.rollOutNumber/2 + floor(obj_info.perp_distance/params.rollOutDensity); + RelativeInfo cur_pose_to_center_line; + PlanningHelpers::GetRelativeInfo(path, currState, cur_pose_to_center_line); + + int currIndex = floor(double(params.rollOutNumber)/2.0 // Center index + + cur_pose_to_center_line.perp_distance/params.rollOutDensity // Normalized distance + + 0.5); + if(currIndex < 0) currIndex = 0; else if(currIndex > params.rollOutNumber) @@ -1041,8 +964,9 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsDynamic(const s { if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, obj_list.at(i).contour.at(icon)) == true) { - for(unsigned int it=0; it< rollOuts.size(); it++) + for(unsigned int it=0; it< rollOuts.size(); it++){ m_TrajectoryCosts.at(it).bBlocked = true; + } return; } diff --git a/autoware.ai/src/autoware/common/ros_observer/launch/test.launch b/autoware.ai/src/autoware/common/ros_observer/launch/test.launch index 45ae3841..68ee5e45 100644 --- a/autoware.ai/src/autoware/common/ros_observer/launch/test.launch +++ b/autoware.ai/src/autoware/common/ros_observer/launch/test.launch @@ -1,6 +1,6 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/autoware_connector/launch/vel_pose_connect.launch b/autoware.ai/src/autoware/core_perception/autoware_connector/launch/vel_pose_connect.launch index 035ea16f..820eea17 100644 --- a/autoware.ai/src/autoware/core_perception/autoware_connector/launch/vel_pose_connect.launch +++ b/autoware.ai/src/autoware/core_perception/autoware_connector/launch/vel_pose_connect.launch @@ -8,13 +8,13 @@ - - + + - - + + diff --git a/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rectifier.launch b/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rectifier.launch index ca0a6265..ed9514e3 100644 --- a/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rectifier.launch +++ b/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rectifier.launch @@ -8,14 +8,14 @@ - + - + - + diff --git a/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rectifier_ladybug.launch b/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rectifier_ladybug.launch index 10150b20..169f401b 100644 --- a/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rectifier_ladybug.launch +++ b/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rectifier_ladybug.launch @@ -3,23 +3,23 @@ - + - + - + - + - + diff --git a/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rotator.launch b/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rotator.launch index 0b772049..278842a9 100644 --- a/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rotator.launch +++ b/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rotator.launch @@ -7,7 +7,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rotator_ladybug.launch b/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rotator_ladybug.launch index f2f455ef..7937054f 100644 --- a/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rotator_ladybug.launch +++ b/autoware.ai/src/autoware/core_perception/image_processor/launch/image_rotator_ladybug.launch @@ -5,31 +5,31 @@ - + - + - + - + - + diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/CMakeLists.txt index 6bf3de1d..a3d5444e 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/CMakeLists.txt +++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/CMakeLists.txt @@ -21,6 +21,7 @@ find_package(catkin REQUIRED COMPONENTS tf vector_map_server rubis_lib + rubis_msgs ) find_package(OpenMP) diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/include/cluster.h b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/include/cluster.h index 26b5b2f4..6ec25fb2 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/include/cluster.h +++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/include/cluster.h @@ -43,7 +43,6 @@ #include #include -#include #include "autoware_msgs/CloudCluster.h" diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/euclidean_clustering_Exp.launch b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/euclidean_clustering_Exp.launch index 992bfee1..1eabfd76 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/euclidean_clustering_Exp.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/euclidean_clustering_Exp.launch @@ -23,7 +23,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect.launch b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect.launch index 88c8ff40..fef67e40 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect.launch @@ -33,7 +33,7 @@ - + @@ -64,7 +64,7 @@ + ns="/detection/lidar_detector"> diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect_param.launch b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect_param.launch index 8ad4a0aa..54915cd9 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect_param.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/launch/lidar_euclidean_cluster_detect_param.launch @@ -4,26 +4,26 @@ + name="lidar_euclidean_cluster_detect"> + ns="/detection/lidar_detector"> + ns="/detection/lidar_detector"> + ns="/detection/lidar_detector"> diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp index 40e5d6e8..30aa9107 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp +++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp @@ -53,6 +53,9 @@ #include "autoware_msgs/DetectedObject.h" #include "autoware_msgs/DetectedObjectArray.h" +#include "rubis_msgs/PointCloud2.h" +#include "rubis_msgs/DetectedObjectArray.h" + #include #include @@ -91,6 +94,8 @@ ros::Publisher _pub_points_lanes_cloud; ros::Publisher _pub_detected_objects; +ros::Publisher _pub_rubis_detected_objects; + std_msgs::Header _velodyne_header; std::string _output_frame; @@ -203,6 +208,7 @@ void transformBoundingBox(const jsk_recognition_msgs::BoundingBox &in_boundingbo void publishDetectedObjects(const autoware_msgs::CloudClusterArray &in_clusters) { autoware_msgs::DetectedObjectArray detected_objects; + rubis_msgs::DetectedObjectArray rubis_detected_objects; detected_objects.header = in_clusters.header; for (size_t i = 0; i < in_clusters.clusters.size(); i++) @@ -221,6 +227,11 @@ void publishDetectedObjects(const autoware_msgs::CloudClusterArray &in_clusters) detected_objects.objects.push_back(detected_object); } _pub_detected_objects.publish(detected_objects); + + rubis_detected_objects.instance = rubis::instance_; + rubis_detected_objects.object_array = detected_objects; + + _pub_rubis_detected_objects.publish(rubis_detected_objects); if(rubis::sched::is_task_ready_ == TASK_NOT_READY){ rubis::sched::init_task(); @@ -681,25 +692,6 @@ void segmentByDistance(const pcl::PointCloud::Ptr in_cloud_ptr, jsk_recognition_msgs::BoundingBox bounding_box = final_clusters[i]->GetBoundingBox(); geometry_msgs::PolygonStamped polygon = final_clusters[i]->GetPolygon(); - jsk_rviz_plugins::Pictogram pictogram_cluster; - pictogram_cluster.header = _velodyne_header; - - // PICTO - pictogram_cluster.mode = pictogram_cluster.STRING_MODE; - pictogram_cluster.pose.position.x = final_clusters[i]->GetMaxPoint().x; - pictogram_cluster.pose.position.y = final_clusters[i]->GetMaxPoint().y; - pictogram_cluster.pose.position.z = final_clusters[i]->GetMaxPoint().z; - tf::Quaternion quat(0.0, -0.7, 0.0, 0.7); - tf::quaternionTFToMsg(quat, pictogram_cluster.pose.orientation); - pictogram_cluster.size = 4; - std_msgs::ColorRGBA color; - color.a = 1; - color.r = 1; - color.g = 1; - color.b = 1; - pictogram_cluster.color = color; - pictogram_cluster.character = std::to_string(i); - // PICTO // pcl::PointXYZ min_point = final_clusters[i]->GetMinPoint(); // pcl::PointXYZ max_point = final_clusters[i]->GetMaxPoint(); @@ -861,8 +853,9 @@ void removePointsUpTo(const pcl::PointCloud::Ptr in_cloud_ptr, } } -void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud) +void velodyne_callback(const rubis_msgs::PointCloud2ConstPtr& in_sensor_cloud) { + rubis::instance_ = in_sensor_cloud->instance; //_start = std::chrono::system_clock::now(); if (!_using_sensor_cloud) { @@ -881,9 +874,9 @@ void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud) autoware_msgs::Centroids centroids; autoware_msgs::CloudClusterArray cloud_clusters; - pcl::fromROSMsg(*in_sensor_cloud, *current_sensor_cloud_ptr); + pcl::fromROSMsg(in_sensor_cloud->msg, *current_sensor_cloud_ptr); - _velodyne_header = in_sensor_cloud->header; + _velodyne_header = in_sensor_cloud->msg.header; if (_remove_points_upto > 0.0) { @@ -985,27 +978,13 @@ int main(int argc, char **argv) generateColors(_colors, 255); - - /* For GPU scheduling */ - #ifdef GPU_CLUSTERING - int key_id = 1; if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); - if(gpu_profiling_flag) rubis::sched::init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename); - - if(gpu_scheduling_flag){ - rubis::sched::init_gpu_scheduling("/tmp/cluster", gpu_deadline_filename, key_id); - } - else if(gpu_scheduling_flag){ - ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!"); - exit(1); - } - #endif - std::string label; std::string output_lane_str; std::string output_cluster_str; std::string output_objects_str; + std::string rubis_output_objects_str; std::string point_cluster_str; std::string cluster_centeroids_str; std::string points_ground_str; @@ -1014,6 +993,7 @@ int main(int argc, char **argv) output_lane_str = std::string("/points_lanes_")+label; output_cluster_str = std::string("/detection/lidar_detector/cloud_clusters_")+label; output_objects_str = std::string("/detection/lidar_detector/objects_")+label; + rubis_output_objects_str = std::string("/detection/lidar_detector/rubis_objects_")+label; point_cluster_str = std::string("/points_cluster_")+label; cluster_centeroids_str = std::string("/points_ground_")+label; points_ground_str = std::string("/cluster_centroids_")+label; @@ -1024,6 +1004,7 @@ int main(int argc, char **argv) _pub_points_lanes_cloud = h.advertise(output_lane_str.c_str(), 1); _pub_clusters_message = h.advertise(output_cluster_str.c_str(), 1); _pub_detected_objects = h.advertise(output_objects_str.c_str(), 1); + _pub_rubis_detected_objects = h.advertise(rubis_output_objects_str.c_str(), 1); std::string points_topic, gridmap_topic; @@ -1154,7 +1135,7 @@ int main(int argc, char **argv) ros::spinOnce(); - if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); + if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); if(rubis::sched::task_state_ == TASK_STATE_DONE){ if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::finish_job(); if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/package.xml b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/package.xml index 00901300..7a07b2c3 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/package.xml +++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/package.xml @@ -22,4 +22,5 @@ tf vector_map_server rubis_lib + rubis_msgs diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/CMakeLists.txt index 691c2316..28d2c31f 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/CMakeLists.txt +++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS tf vector_map rubis_lib + rubis_msgs ) diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/include/imm_ukf_pda/imm_ukf_pda.h b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/include/imm_ukf_pda/imm_ukf_pda.h index 29b02a5a..e386b8df 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/include/imm_ukf_pda/imm_ukf_pda.h +++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/include/imm_ukf_pda/imm_ukf_pda.h @@ -38,6 +38,8 @@ #include "autoware_msgs/DetectedObject.h" #include "autoware_msgs/DetectedObjectArray.h" +#include "rubis_msgs/DetectedObjectArray.h" + #include "ukf.h" extern int is_topic_ready; @@ -90,7 +92,9 @@ class ImmUkfPda const double CENTROID_DISTANCE = 0.2;//distance to consider centroids the same std::string input_topic_; + std::string input_rubis_topic_; std::string output_topic_; + std::string output_rubis_topic_; std::string tracking_frame_; @@ -102,12 +106,16 @@ class ImmUkfPda ros::NodeHandle node_handle_; ros::NodeHandle private_nh_; ros::Subscriber sub_detected_array_; + ros::Subscriber sub_rubis_detected_array_; ros::Publisher pub_object_array_; + ros::Publisher pub_rubis_object_array_; std_msgs::Header input_header_; void callback(const autoware_msgs::DetectedObjectArray& input); + void rubis_callback(const rubis_msgs::DetectedObjectArray& input); + void transformPoseToGlobal(const autoware_msgs::DetectedObjectArray& input, autoware_msgs::DetectedObjectArray& transformed_input); void transformPoseToLocal(autoware_msgs::DetectedObjectArray& detected_objects_output); diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch index 0ab3cb8d..77f64c75 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch @@ -4,6 +4,8 @@ + + @@ -23,9 +25,11 @@ - + + + @@ -46,12 +50,12 @@ + ns="$(arg namespace)"> + args="$(arg namespace)/objects_$(arg label) /detection/objects_$(arg label)"/> --> diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track_lanelet2.launch b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track_lanelet2.launch index d7ebdafc..358b6002 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track_lanelet2.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track_lanelet2.launch @@ -22,7 +22,7 @@ - + @@ -43,9 +43,9 @@ + ns="$(arg namespace)"/> + args="$(arg namespace)/objects /detection/objects"/> diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track_option.launch b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track_option.launch index c8889fc8..f3dbf8ab 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track_option.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track_option.launch @@ -24,7 +24,7 @@ - + @@ -46,7 +46,7 @@ - + @@ -69,9 +69,9 @@ + ns="$(arg namespace)"/> + args="$(arg namespace)/objects /detection/objects"/> diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp index 131e0f1e..e2e69cef 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp +++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp @@ -54,13 +54,17 @@ ImmUkfPda::ImmUkfPda() // topic name private_nh_.param("tracker_input_topic", input_topic_, "/detection/lidar_detector/objects"); + private_nh_.param("tracker_input_rubis_topic", input_rubis_topic_, "/detection/lidar_detector/rubis_objects_center"); private_nh_.param("tracker_output_topic", output_topic_, "/detection/object_tracker/objects"); + private_nh_.param("tracker_output_rubis_topic", output_rubis_topic_, "/detection/object_tracker/rubis_objects_center"); } void ImmUkfPda::run() { pub_object_array_ = node_handle_.advertise(output_topic_.c_str(), 1); sub_detected_array_ = node_handle_.subscribe(input_topic_.c_str(), 1, &ImmUkfPda::callback, this); + pub_rubis_object_array_ = node_handle_.advertise(output_rubis_topic_.c_str(), 1); + sub_rubis_detected_array_ = node_handle_.subscribe(input_rubis_topic_.c_str(), 1, &ImmUkfPda::rubis_callback, this); if (use_vectormap_) { @@ -70,6 +74,43 @@ void ImmUkfPda::run() } } +void ImmUkfPda::rubis_callback(const rubis_msgs::DetectedObjectArray& input) +{ + rubis::instance_ = input.instance; + input_header_ = input.object_array.header; + + if(use_vectormap_) + { + checkVectormapSubscription(); + } + + bool success = updateNecessaryTransform(); + if (!success) + { + ROS_INFO("Could not find coordiante transformation"); + return; + } + + autoware_msgs::DetectedObjectArray transformed_input; + autoware_msgs::DetectedObjectArray detected_objects_output; + rubis_msgs::DetectedObjectArray rubis_detected_objects_output; + transformPoseToGlobal(input.object_array, transformed_input); + tracker(transformed_input, detected_objects_output); + transformPoseToLocal(detected_objects_output); + + rubis_detected_objects_output.instance = rubis::instance_; + rubis_detected_objects_output.object_array = detected_objects_output; + pub_rubis_object_array_.publish(rubis_detected_objects_output); + + if (is_benchmark_) + { + dumpResultText(detected_objects_output); + } + + if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); + rubis::sched::task_state_ = TASK_STATE_DONE; +} + void ImmUkfPda::callback(const autoware_msgs::DetectedObjectArray& input) { input_header_ = input.header; @@ -98,9 +139,6 @@ void ImmUkfPda::callback(const autoware_msgs::DetectedObjectArray& input) { dumpResultText(detected_objects_output); } - - if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); - rubis::sched::task_state_ = TASK_STATE_DONE; } void ImmUkfPda::checkVectormapSubscription() diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp index 6598327b..8d99fbef 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp +++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp @@ -68,7 +68,7 @@ int main(int argc, char** argv) ros::spinOnce(); - if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); + if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); if(rubis::sched::task_state_ == TASK_STATE_DONE){ if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/package.xml b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/package.xml index 0dd5b97c..84beb6fb 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/package.xml +++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/package.xml @@ -13,9 +13,10 @@ amathutils_lib pcl_ros roscpp - roslint + roslint tf vector_map lanelet2_extension rubis_lib + rubis_msgs diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_mapping.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_mapping.launch index eda6827e..932ad3fe 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_mapping.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_mapping.launch @@ -9,8 +9,8 @@ - - + + diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_mapping_tku.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_mapping_tku.launch index 227379ba..ad6e9e76 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_mapping_tku.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_mapping_tku.launch @@ -9,7 +9,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch index 20f7d298..b0642a01 100755 --- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch @@ -25,7 +25,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_monitor.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_monitor.launch index 4678d69f..0881b8d1 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_monitor.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_monitor.launch @@ -6,7 +6,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch index 1b882f79..ebf098ff 100755 --- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch @@ -25,7 +25,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_tku.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_tku.launch index e52ff085..1da32d06 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_tku.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_tku.launch @@ -9,7 +9,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_tku_org.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_tku_org.launch index 7a99aa13..42473e1b 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_tku_org.launch +++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_tku_org.launch @@ -9,7 +9,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp index 92a4f873..b7c00acb 100644 --- a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp +++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp @@ -48,6 +48,8 @@ #include #include +#include + #include #include #include @@ -74,9 +76,6 @@ #include -//headers in Autoware Health Checker -#include - #include #include #include @@ -95,8 +94,6 @@ #define DEBUG -static std::shared_ptr health_checker_ptr_; - struct pose { double x; @@ -177,8 +174,9 @@ static ros::Publisher current_pose_pub; static geometry_msgs::PoseStamped current_pose_msg; */ -static ros::Publisher localizer_pose_pub; +static ros::Publisher localizer_pose_pub, rubis_localizer_pose_pub; static geometry_msgs::PoseStamped localizer_pose_msg; +static rubis_msgs::PoseStamped rubis_localizer_pose_msg; static ros::Publisher estimate_twist_pub; static geometry_msgs::TwistStamped estimate_twist_msg; @@ -991,8 +989,6 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) } } - health_checker_ptr_->CHECK_RATE("topic_rate_filtered_points_slow", 8, 5, 1, "topic filtered_points subscribe rate slow."); - matching_start = std::chrono::system_clock::now(); static tf::TransformBroadcaster br, kalman_br; @@ -1505,7 +1501,6 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) } predict_pose_pub.publish(predict_pose_msg); - health_checker_ptr_->CHECK_RATE("topic_rate_ndt_pose_slow", 8, 5, 1, "topic ndt_pose publish rate slow."); ndt_pose_pub.publish(ndt_pose_msg); rubis::sched::task_state_ = TASK_STATE_DONE; @@ -1513,16 +1508,21 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) rubis_ndt_pose_msg.instance = rubis::instance_; rubis_ndt_pose_msg.msg = ndt_pose_msg; rubis_ndt_pose_pub.publish(rubis_ndt_pose_msg); + + rubis_msgs::PoseStamped rubis_localizer_pose_msg; + rubis_localizer_pose_msg.instance = rubis::instance_; + rubis_localizer_pose_msg.msg = localizer_pose_msg; + rubis_localizer_pose_pub.publish(rubis_localizer_pose_msg); } // current_pose is published by vel_pose_mux // current_pose_pub.publish(current_pose_msg); - localizer_pose_pub.publish(localizer_pose_msg); + localizer_pose_pub.publish(localizer_pose_msg); + matching_end = std::chrono::system_clock::now(); exe_time = std::chrono::duration_cast(matching_end - matching_start).count() / 1000.0; time_ndt_matching.data = exe_time; - health_checker_ptr_->CHECK_MAX_VALUE("time_ndt_matching", time_ndt_matching.data, 50, 70, 100, "value time_ndt_matching is too high."); time_ndt_matching_pub.publish(time_ndt_matching); // Set values for /estimate_twist @@ -1540,8 +1540,6 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) geometry_msgs::Vector3Stamped estimate_vel_msg; estimate_vel_msg.header.stamp = current_scan_time; estimate_vel_msg.vector.x = current_velocity; - health_checker_ptr_->CHECK_MAX_VALUE("estimate_twist_linear", current_velocity, 5, 10, 15, "value linear estimated twist is too high."); - health_checker_ptr_->CHECK_MAX_VALUE("estimate_twist_angular", angular_velocity, 5, 10, 15, "value linear angular twist is too high."); estimated_vel_pub.publish(estimate_vel_msg); previous_score = fitness_score; @@ -1770,9 +1768,6 @@ int main(int argc, char** argv) ros::NodeHandle nh; ros::NodeHandle private_nh("~"); - health_checker_ptr_ = std::make_shared(nh,private_nh); - health_checker_ptr_->ENABLE(); - health_checker_ptr_->NODE_ACTIVATE(); // Set log file name. private_nh.getParam("output_log_data", _output_log_data); @@ -1970,9 +1965,11 @@ int main(int argc, char** argv) ndt_pose_pub = nh.advertise("/ndt_pose", 10); if(rubis::instance_mode_) rubis_ndt_pose_pub = nh.advertise("/rubis_ndt_pose",10); - // current_pose_pub = nh.advertise("/current_pose", 10); - localizer_pose_pub = nh.advertise("/localizer_pose", 10); - estimate_twist_pub = nh.advertise("/estimate_twist", 10); + localizer_pose_pub = nh.advertise("/current_pose", 10); + rubis_localizer_pose_pub = nh.advertise("/rubis_current_pose", 10); + // localizer_pose_pub = nh.advertise("/localizer_pose", 10); + estimate_twist_pub = nh.advertise("/current_velocity", 10); + // estimate_twist_pub = nh.advertise("/estimate_twist", 10); estimated_vel_mps_pub = nh.advertise("/estimated_vel_mps", 10); estimated_vel_kmph_pub = nh.advertise("/estimated_vel_kmph", 10); estimated_vel_pub = nh.advertise("/estimated_vel", 10); diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp new file mode 100644 index 00000000..b931fd74 --- /dev/null +++ b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp @@ -0,0 +1,211 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include + +#include "autoware_config_msgs/ConfigVoxelGridFilter.h" + +#include + +#include + +#include "points_downsampler.h" + +#include +#include + +#define SPIN_PROFILING + +#define MAX_MEASUREMENT_RANGE 200.0 + +ros::Publisher filtered_points_pub; +ros::Publisher rubis_filtered_points_pub; + +// Leaf size of VoxelGrid filter. +static double voxel_leaf_size = 2.0; + +static ros::Publisher points_downsampler_info_pub; +static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; + +static std::chrono::time_point filter_start, filter_end; + +static bool _output_log = false; +static std::ofstream ofs; +static std::string filename; + +static std::string input_topic_name_; +static std::string output_topic_name_; +static double measurement_range = MAX_MEASUREMENT_RANGE; + +static void config_callback(const autoware_config_msgs::ConfigVoxelGridFilter::ConstPtr& input) +{ + voxel_leaf_size = input->voxel_leaf_size; + measurement_range = input->measurement_range; +} + +inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstPtr& input) +{ + pcl::PointCloud scan; + pcl::fromROSMsg(*input, scan); + + if(measurement_range != MAX_MEASUREMENT_RANGE){ + scan = removePointsByRange(scan, 0, measurement_range); + } + + pcl::PointCloud::Ptr scan_ptr(new pcl::PointCloud(scan)); + pcl::PointCloud::Ptr filtered_scan_ptr(new pcl::PointCloud()); + + sensor_msgs::PointCloud2 filtered_msg; + + filter_start = std::chrono::system_clock::now(); + + // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL) + if (voxel_leaf_size >= 0.1) + { + // Downsampling the velodyne scan using VoxelGrid filter + pcl::VoxelGrid voxel_grid_filter; + voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); + voxel_grid_filter.setInputCloud(scan_ptr); + voxel_grid_filter.filter(*filtered_scan_ptr); + pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); + } + else + { + pcl::toROSMsg(*scan_ptr, filtered_msg); + } + + filter_end = std::chrono::system_clock::now(); + + filtered_msg.header = input->header; + filtered_points_pub.publish(filtered_msg); + + rubis_msgs::PointCloud2 rubis_filtered_msg; + rubis_filtered_msg.msg = filtered_msg; + rubis_filtered_msg.instance = rubis::instance_; + rubis_filtered_points_pub.publish(rubis_filtered_msg); + + points_downsampler_info_msg.header = input->header; + points_downsampler_info_msg.filter_name = "voxel_grid_filter"; + points_downsampler_info_msg.measurement_range = measurement_range; + points_downsampler_info_msg.original_points_size = scan.size(); + if (voxel_leaf_size >= 0.1) + { + points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); + } + else + { + points_downsampler_info_msg.filtered_points_size = scan_ptr->size(); + } + points_downsampler_info_msg.original_ring_size = 0; + points_downsampler_info_msg.filtered_ring_size = 0; + points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; + points_downsampler_info_pub.publish(points_downsampler_info_msg); + + if(_output_log == true){ + if(!ofs){ + std::cerr << "Could not open " << filename << "." << std::endl; + exit(1); + } + ofs << points_downsampler_info_msg.header.seq << "," + << points_downsampler_info_msg.header.stamp << "," + << points_downsampler_info_msg.header.frame_id << "," + << points_downsampler_info_msg.filter_name << "," + << points_downsampler_info_msg.original_points_size << "," + << points_downsampler_info_msg.filtered_points_size << "," + << points_downsampler_info_msg.original_ring_size << "," + << points_downsampler_info_msg.filtered_ring_size << "," + << points_downsampler_info_msg.exe_time << "," + << std::endl; + } + +} + +static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) +{ + rubis::instance_ = 0; + publish_filtered_cloud(input); +} + +static void rubis_scan_callback(const rubis_msgs::PointCloud2::ConstPtr& _input) +{ + rubis::start_task_profiling(); + + sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared(_input->msg); + rubis::instance_ = _input->instance; + publish_filtered_cloud(input); + + rubis::stop_task_profiling(rubis::instance_, 0); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "rubis_voxel_grid_filter"); + + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + + // Scheduling Setup + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; + double task_execution_time; + double task_relative_deadline; + + private_nh.param("input_topic_name", input_topic_name_, std::string("points_raw")); + private_nh.param("output_topic_name", output_topic_name_, std::string("filtered_points")); + private_nh.getParam("output_log", _output_log); + if(_output_log == true){ + char buffer[80]; + std::time_t now = std::time(NULL); + std::tm *pnow = std::localtime(&now); + std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); + filename = "voxel_grid_filter_" + std::string(buffer) + ".csv"; + ofs.open(filename.c_str(), std::ios::app); + } + private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); + private_nh.param("leaf_size", voxel_leaf_size, 0.1); + + std::string node_name = ros::this_node::getName(); + private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voxel_grid_filter.csv"); + private_nh.param(node_name+"/rate", rate, 10); + private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10); + private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); + + /* For Task scheduling */ + rubis::init_task_profiling(task_response_time_filename); + + // Publishers + filtered_points_pub = nh.advertise(output_topic_name_, 10); + rubis_filtered_points_pub = nh.advertise("/rubis_" + output_topic_name_, 10); + points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); + + // Subscribers + ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback); + // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback); + ros::Subscriber scan_sub; + + scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 10, rubis_scan_callback); + + ros::spin(); + + return 0; +} diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h b/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h index 58e1bafa..18298682 100644 --- a/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h +++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h @@ -33,14 +33,13 @@ #include #include #include "autoware_config_msgs/ConfigRayGroundFilter.h" +#include "rubis_msgs/PointCloud2.h" #include #include #include // headers in Autoware Health Checker -#include - #include #include "gencolors.cpp" @@ -51,7 +50,6 @@ class RayGroundFilter { private: - std::shared_ptr health_checker_ptr_; ros::NodeHandle node_handle_; ros::Subscriber points_node_sub_; ros::Subscriber config_node_sub_; diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/cloud_transformer.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/cloud_transformer.launch index 23e1c671..cf206b9b 100755 --- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/cloud_transformer.launch +++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/cloud_transformer.launch @@ -6,7 +6,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/compare_map_filter_params.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/compare_map_filter_params.launch index dedfc7a7..c5f8fdd8 100755 --- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/compare_map_filter_params.launch +++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/compare_map_filter_params.launch @@ -4,7 +4,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/points_concat_filter.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/points_concat_filter.launch index 3a4476bc..810c802b 100755 --- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/points_concat_filter.launch +++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/points_concat_filter.launch @@ -5,7 +5,7 @@ + name="points_concat_filter"> diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch index 45d448f1..25fbed0c 100755 --- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch +++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch @@ -17,7 +17,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp index bc9dd3de..005e14fb 100644 --- a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp +++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp @@ -98,7 +98,11 @@ void RayGroundFilter::publish_cloud(const ros::Publisher& in_publisher, << in_header.frame_id); return; } - in_publisher.publish(*trans_cloud_msg_ptr); + + rubis_msgs::PointCloud2 msg; + msg.instance = rubis::instance_; + msg.msg = *trans_cloud_msg_ptr; + in_publisher.publish(msg); } /*! @@ -329,9 +333,6 @@ void RayGroundFilter::RemovePointsUpTo(const pcl::PointCloud::Pt inline void RayGroundFilter::PublishFilteredClouds(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud){ - health_checker_ptr_->NODE_ACTIVATE(); - health_checker_ptr_->CHECK_RATE("topic_rate_points_raw_slow", 8, 5, 1, "topic points_raw subscribe rate slow."); - sensor_msgs::PointCloud2::Ptr trans_sensor_cloud(new sensor_msgs::PointCloud2); const bool succeeded = TransformPointCloud(base_frame_, in_sensor_cloud, trans_sensor_cloud); @@ -391,6 +392,7 @@ void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_s void RayGroundFilter::RubisCloudCallback(const rubis_msgs::PointCloud2ConstPtr in_rubis_cloud) { sensor_msgs::PointCloud2ConstPtr in_sensor_cloud = boost::make_shared(in_rubis_cloud->msg); + rubis::instance_ = in_rubis_cloud->instance; PublishFilteredClouds(in_sensor_cloud); } @@ -399,8 +401,6 @@ RayGroundFilter::RayGroundFilter() : node_handle_("~"), tf_listener_(tf_buffer_) { ros::NodeHandle nh; ros::NodeHandle pnh("~"); - health_checker_ptr_ = std::make_shared(nh, pnh); - health_checker_ptr_->ENABLE(); } void RayGroundFilter::Run() @@ -466,7 +466,7 @@ void RayGroundFilter::Run() config_node_sub_ = node_handle_.subscribe("/config/ray_ground_filter", 1, &RayGroundFilter::update_config_params, this); - groundless_points_pub_ = node_handle_.advertise(no_ground_topic, 2); + groundless_points_pub_ = node_handle_.advertise(no_ground_topic, 2); ground_points_pub_ = node_handle_.advertise(ground_topic, 2); std::string node_name = ros::this_node::getName(); @@ -479,8 +479,7 @@ void RayGroundFilter::Run() node_handle_.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); node_handle_.param(node_name+"/instance_mode", instance_mode_, 0); - if(instance_mode_) points_node_sub_ = node_handle_.subscribe("/rubis_"+input_point_topic_.substr(1), 1, &RayGroundFilter::RubisCloudCallback, this); - else points_node_sub_ = node_handle_.subscribe(input_point_topic_, 1, &RayGroundFilter::CloudCallback, this); + points_node_sub_ = node_handle_.subscribe("/rubis_"+input_point_topic_.substr(1), 1, &RayGroundFilter::RubisCloudCallback, this); ROS_INFO("Ready"); @@ -508,7 +507,7 @@ void RayGroundFilter::Run() ros::spinOnce(); - if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); + if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); if(rubis::sched::task_state_ == TASK_STATE_DONE){ if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); rubis::sched::task_state_ = TASK_STATE_READY; diff --git a/autoware.ai/src/autoware/core_perception/range_vision_fusion/launch/range_vision_fusion.launch b/autoware.ai/src/autoware/core_perception/range_vision_fusion/launch/range_vision_fusion.launch index 0ff66af8..41579902 100644 --- a/autoware.ai/src/autoware/core_perception/range_vision_fusion/launch/range_vision_fusion.launch +++ b/autoware.ai/src/autoware/core_perception/range_vision_fusion/launch/range_vision_fusion.launch @@ -12,7 +12,7 @@ - + @@ -26,21 +26,21 @@ + ns="$(arg namespace)"> diff --git a/autoware.ai/src/autoware/core_perception/twist_generator/launch/vehicle_status_converter.launch b/autoware.ai/src/autoware/core_perception/twist_generator/launch/vehicle_status_converter.launch index 1e50181d..fca05249 100644 --- a/autoware.ai/src/autoware/core_perception/twist_generator/launch/vehicle_status_converter.launch +++ b/autoware.ai/src/autoware/core_perception/twist_generator/launch/vehicle_status_converter.launch @@ -5,7 +5,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/vision_beyond_track/launch/vision_beyond_track.launch b/autoware.ai/src/autoware/core_perception/vision_beyond_track/launch/vision_beyond_track.launch index b07a0716..c9df1538 100644 --- a/autoware.ai/src/autoware/core_perception/vision_beyond_track/launch/vision_beyond_track.launch +++ b/autoware.ai/src/autoware/core_perception/vision_beyond_track/launch/vision_beyond_track.launch @@ -3,7 +3,7 @@ - + diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_darknet_detect_parameter.launch b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_darknet_detect_parameter.launch index a79ffa80..fae75884 100644 --- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_darknet_detect_parameter.launch +++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_darknet_detect_parameter.launch @@ -7,7 +7,7 @@ - + @@ -16,7 +16,7 @@ + > diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo2_detect.launch b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo2_detect.launch index 6fc73fc4..08d93182 100644 --- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo2_detect.launch +++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo2_detect.launch @@ -10,7 +10,7 @@ - + @@ -21,7 +21,7 @@ + > diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo3_detect.launch b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo3_detect.launch index e1376f17..deab84ba 100644 --- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo3_detect.launch +++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/launch/vision_yolo3_detect.launch @@ -9,7 +9,7 @@ - + @@ -20,7 +20,7 @@ + > diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/launch/op_global_planner.launch b/autoware.ai/src/autoware/core_planning/op_global_planner/launch/op_global_planner.launch index de166464..b8a99219 100644 --- a/autoware.ai/src/autoware/core_planning/op_global_planner/launch/op_global_planner.launch +++ b/autoware.ai/src/autoware/core_planning/op_global_planner/launch/op_global_planner.launch @@ -20,7 +20,7 @@ - + diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp b/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp index baa5bc9d..67a65d23 100644 --- a/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp +++ b/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp @@ -260,8 +260,8 @@ bool GlobalPlanner::GenerateGlobalPlan(PlannerHNS::WayPoint& startPoint, Planner if(ret == 0) { - std::cout << "Can't Generate Global Path for Start (" << startPoint.pos.ToString() - << ") and Goal (" << goalPoint.pos.ToString() << ")" << std::endl; + // std::cout << "Can't Generate Global Path for Start (" << startPoint.pos.ToString() + // << ") and Goal (" << goalPoint.pos.ToString() << ")" << std::endl; return false; } @@ -522,16 +522,16 @@ void GlobalPlanner::MainLoop() m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_params.bEnableLaneChange, false); - XmlRpc::XmlRpcValue lane_info_xml; - // Add Lane Info from yaml file if exist - try{ - nh.getParam("/op_global_planner/lane_info_list", lane_info_xml); - } - catch(XmlRpc::XmlRpcException& e){ - ROS_WARN("No Lane Info yaml file"); - lane_info_xml.clear(); - } - PlannerHNS::MappingHelpers::ConstructLaneInfo_RUBIS(m_Map, lane_info_xml); + // XmlRpc::XmlRpcValue lane_info_xml; + // // Add Lane Info from yaml file if exist + // try{ + // nh.getParam("/op_global_planner/lane_info_list", lane_info_xml); + // } + // catch(XmlRpc::XmlRpcException& e){ + // ROS_WARN("No Lane Info yaml file"); + // lane_info_xml.clear(); + // } + // PlannerHNS::MappingHelpers::ConstructLaneInfo_RUBIS(m_Map, lane_info_xml); } else if(m_MapRaw.GetVersion()==1) { diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/op_local_planner/CMakeLists.txt index cbec051b..c4698aea 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/CMakeLists.txt +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/CMakeLists.txt @@ -25,6 +25,7 @@ find_package( tf vector_map_msgs rubis_lib + rubis_msgs ) find_package(PkgConfig REQUIRED) diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h index 42eab8ba..3890c27d 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h @@ -50,6 +50,8 @@ #include "op_planner/BehaviorPrediction.h" #include "op_utility/DataRW.h" +#include "rubis_msgs/DetectedObjectArray.h" + namespace MotionPredictorNS { @@ -101,7 +103,7 @@ class MotionPrediction timespec m_SensingTimer; // Object Msg List - std::vector object_msg_list_; + std::vector object_msg_list_; // TF std::vector tf_str_list_; @@ -110,6 +112,7 @@ class MotionPrediction ros::NodeHandle nh; ros::Publisher pub_predicted_objects_trajectories; + ros::Publisher pub_rubis_predicted_objects_trajectories; ros::Publisher pub_PredictedTrajectoriesRviz ; ros::Publisher pub_CurbsRviz ; ros::Publisher pub_ParticlesRviz; @@ -124,6 +127,7 @@ class MotionPrediction // Callback function for subscriber. void callbackGetTrackedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg); + void callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg); void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg); void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h index 828e6ea3..f2c4576d 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h @@ -40,6 +40,7 @@ #include "op_planner/PlannerCommonDef.h" #include "op_planner/TrajectoryDynamicCosts.h" +#include "rubis_msgs/DetectedObjectArray.h" @@ -115,6 +116,7 @@ class TrajectoryEval ros::Subscriber sub_GlobalPlannerPaths; ros::Subscriber sub_LocalPlannerPaths; ros::Subscriber sub_predicted_objects; + ros::Subscriber sub_rubis_predicted_objects; ros::Subscriber sub_current_behavior; // HJW added @@ -135,6 +137,7 @@ class TrajectoryEval void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); void callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); void callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg); + void callbackGetRubisPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg); void callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr & msg); void callbackGetCurrentState(const std_msgs::Int32 & msg); diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch index 2af9250d..0e794331 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch @@ -12,7 +12,7 @@ - + diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch index 7c0bf9ac..7899250e 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch @@ -8,7 +8,7 @@ - + diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params.launch index 6e095350..0f6880e5 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params.launch +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params.launch @@ -52,7 +52,7 @@ - + diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params_parameter.launch index 589dc93c..40b901f5 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params_parameter.launch +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_common_params_parameter.launch @@ -52,7 +52,7 @@ - + diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_motion_predictor.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_motion_predictor.launch index 123351f3..cb56dec8 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_motion_predictor.launch +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_motion_predictor.launch @@ -12,7 +12,7 @@ - + @@ -26,7 +26,7 @@ + > diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch index 37026b00..f516d118 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch @@ -1,6 +1,7 @@ - + + @@ -11,6 +12,10 @@ + + + + @@ -25,7 +30,8 @@ - + + @@ -34,7 +40,11 @@ - + + + + + diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch index 2e79bd1e..45cea5b2 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch @@ -1,16 +1,16 @@ + - - - - - + + + + @@ -21,12 +21,11 @@ + - - - - - + + + diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch.backup b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch.backup deleted file mode 100644 index a8ffc17c..00000000 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch.backup +++ /dev/null @@ -1,49 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp new file mode 100644 index 00000000..250edb07 --- /dev/null +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp @@ -0,0 +1,905 @@ +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "op_behavior_selector_core.h" +#include "op_ros_helpers/op_ROSHelpers.h" +#include "op_planner/MappingHelpers.h" +#include + +namespace BehaviorGeneratorNS +{ + +BehaviorGen::BehaviorGen() +{ + sleep(2); + bNewCurrentPos = false; + bVehicleStatus = false; + bWayGlobalPath = false; + bWayGlobalPathLogs = false; + bNewLightStatus = false; + bNewLightSignal = false; + bBestCost = false; + bMap = false; + bRollOuts = false; + UtilityHNS::UtilityH::GetTickCount(planningTimer); + + ros::NodeHandle _nh; + UpdatePlanningParams(_nh); + + // RUBIS driving parameter + nh.getParam("/op_behavior_selector/distanceToPedestrianThreshold", m_distanceToPedestrianThreshold); + nh.param("/op_behavior_selector/turnThreshold", m_turnThreshold, 20.0); + + + tf::StampedTransform transform; + PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); + m_OriginPos.position.x = transform.getOrigin().x(); + m_OriginPos.position.y = transform.getOrigin().y(); + m_OriginPos.position.z = transform.getOrigin().z(); + + pub_LocalPath = nh.advertise("final_waypoints", 1,true); + pub_LocalPathWithPosePub = nh.advertise("final_waypoints_with_pose_twist", 1,true); + pub_LocalBasePath = nh.advertise("base_waypoints", 1,true); + pub_ClosestIndex = nh.advertise("closest_waypoint", 1,true); + pub_BehaviorState = nh.advertise("current_behavior", 1); + pub_SimuBoxPose = nh.advertise("sim_box_pose_ego", 1); + pub_BehaviorStateRviz = nh.advertise("behavior_state", 1); + pub_SelectedPathRviz = nh.advertise("local_selected_trajectory_rviz", 1); + pub_EmergencyStop = nh.advertise("emergency_stop", 1); + pub_turnAngle = nh.advertise("turn_angle", 1); + pub_turnMarker = nh.advertise("turn_marker", 1); + pub_currentState = nh.advertise("current_state", 1); + + int bVelSource = 1; + _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); + if(bVelSource == 0) + sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this); + else if(bVelSource == 2) + sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this); + + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); + sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories_with_pose_twist", 1, &BehaviorGen::callbackGetLocalPlannerPath, this); + // sub_TrafficLightStatus = nh.subscribe("/light_color", 1, &BehaviorGen::callbackGetTrafficLightStatus, this); + // sub_TrafficLightSignals = nh.subscribe("/roi_signal", 1, &BehaviorGen::callbackGetTrafficLightSignals, this); + sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this); + + sub_TrafficLightSignals = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this); + + sub_twist_raw = nh.subscribe("/twist_raw", 1, &BehaviorGen::callbackGetTwistRaw, this); + sub_twist_cmd = nh.subscribe("/twist_cmd", 1, &BehaviorGen::callbackGetTwistCMD, this); + //sub_ctrl_cmd = nh.subscribe("/ctrl_cmd", 1, &BehaviorGen::callbackGetCommandCMD, this); + sub_DistanceToPedestrian = nh.subscribe("/distance_to_pedestrian", 1, &BehaviorGen::callbackDistanceToPedestrian, this); + sub_IntersectionCondition = nh.subscribe("/intersection_condition", 1, &BehaviorGen::callbackIntersectionCondition, this); + sub_SprintSwitch = nh.subscribe("/sprint_switch", 1, &BehaviorGen::callbackSprintSwitch, this); + + //Mapping Section + sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &BehaviorGen::callbackGetVMLanes, this); + sub_points = nh.subscribe("/vector_map_info/point", 1, &BehaviorGen::callbackGetVMPoints, this); + sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &BehaviorGen::callbackGetVMdtLanes, this); + sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &BehaviorGen::callbackGetVMIntersections, this); + sup_area = nh.subscribe("/vector_map_info/area", 1, &BehaviorGen::callbackGetVMAreas, this); + sub_lines = nh.subscribe("/vector_map_info/line", 1, &BehaviorGen::callbackGetVMLines, this); + sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &BehaviorGen::callbackGetVMStopLines, this); + sub_signals = nh.subscribe("/vector_map_info/signal", 1, &BehaviorGen::callbackGetVMSignal, this); + sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &BehaviorGen::callbackGetVMVectors, this); + sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &BehaviorGen::callbackGetVMCurbs, this); + sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &BehaviorGen::callbackGetVMRoadEdges, this); + sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &BehaviorGen::callbackGetVMWayAreas, this); + sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &BehaviorGen::callbackGetVMCrossWalks, this); + sub_nodes = nh.subscribe("/vector_map_info/node", 1, &BehaviorGen::callbackGetVMNodes, this); +} + +BehaviorGen::~BehaviorGen() +{ + UtilityHNS::DataRW::WriteLogData(UtilityHNS::UtilityH::GetHomeDirectory()+UtilityHNS::DataRW::LoggingMainfolderName+UtilityHNS::DataRW::StatesLogFolderName, "MainLog", + "time,dt, Behavior_i, Behavior_str, RollOuts_n, Blocked_i, Central_i, Selected_i, StopSign_id, Light_id, Stop_Dist, Follow_Dist, Follow_Vel," + "Target_Vel, PID_Vel, T_cmd_Vel, C_cmd_Vel, Vel, Steer, X, Y, Z, Theta," + , m_LogData); +} + +void BehaviorGen::UpdatePlanningParams(ros::NodeHandle& _nh) +{ + _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); + if(m_PlanningParams.enableSwerving) + m_PlanningParams.enableFollowing = true; + else + _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); + + _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); + _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); + + _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); + _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); + _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); + + _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); + + _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); + if(m_PlanningParams.enableSwerving) + _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); + else + m_PlanningParams.rollOutNumber = 0; + + _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); + _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); + _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); + _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); + _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); + + _nh.getParam("/op_trajectory_evaluator/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); + _nh.getParam("/op_trajectory_evaluator/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); + + _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); + + _nh.getParam("/op_common_params/width", m_CarInfo.width); + _nh.getParam("/op_common_params/length", m_CarInfo.length); + _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); + _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); + _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); + _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); + _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); + m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; + m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; + + _nh.getParam("/op_common_params/stopLineMargin", m_PlanningParams.stopLineMargin); + _nh.getParam("/op_common_params/stopLineDetectionDistance", m_PlanningParams.stopLineDetectionDistance); + + PlannerHNS::ControllerParams controlParams; + controlParams.Steering_Gain = PlannerHNS::PID_CONST(0.07, 0.02, 0.01); + controlParams.Velocity_Gain = PlannerHNS::PID_CONST(0.1, 0.005, 0.1); + nh.getParam("/op_common_params/steeringDelay", controlParams.SteeringDelay); + nh.getParam("/op_common_params/minPursuiteDistance", controlParams.minPursuiteDistance ); + nh.getParam("/op_common_params/additionalBrakingDistance", m_PlanningParams.additionalBrakingDistance ); + nh.getParam("/op_common_params/giveUpDistance", m_PlanningParams.giveUpDistance ); + nh.getParam("/op_common_params/enableSlowDownOnCurve", m_PlanningParams.enableSlowDownOnCurve ); + nh.getParam("/op_common_params/curveVelocityRatio", m_PlanningParams.curveVelocityRatio ); + + int iSource = 0; + _nh.getParam("/op_common_params/mapSource" , iSource); + if(iSource == 0) + m_MapType = PlannerHNS::MAP_AUTOWARE; + else if (iSource == 1) + m_MapType = PlannerHNS::MAP_FOLDER; + else if(iSource == 2) + m_MapType = PlannerHNS::MAP_KML_FILE; + + _nh.getParam("/op_common_params/mapFileName" , m_MapPath); + + _nh.getParam("/op_behavior_selector/evidence_tust_number", m_PlanningParams.nReliableCount); + + //std::cout << "nReliableCount: " << m_PlanningParams.nReliableCount << std::endl; + + _nh.param("/op_behavior_selector/sprintSpeed", m_sprintSpeed, 13.5); + _nh.param("/op_behavior_selector/obstacleWaitingTimeinIntersection", m_obstacleWaitingTimeinIntersection, 1.0); + + m_BehaviorGenerator.Init(controlParams, m_PlanningParams, m_CarInfo, m_sprintSpeed); + + m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior = PlannerHNS::INITIAL_STATE; + + m_BehaviorGenerator.m_obstacleWaitingTimeinIntersection = m_obstacleWaitingTimeinIntersection; +} + +void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ + double distance = msg.data; + if(distance < m_distanceToPedestrianThreshold){ + m_PlanningParams.pedestrianAppearence = true; + } + else + { + m_PlanningParams.pedestrianAppearence = false; + } + m_BehaviorGenerator.UpdatePedestrianAppearence(m_PlanningParams.pedestrianAppearence); + // m_BehaviorGenerator.printPedestrianAppearence(); +} + +void BehaviorGen::callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg){ + m_BehaviorGenerator.m_isInsideIntersection = msg.isIntersection; + m_BehaviorGenerator.m_closestIntersectionDistance = msg.intersectionDistance; + m_BehaviorGenerator.m_riskyLeft = msg.riskyLeftTurn; + m_BehaviorGenerator.m_riskyRight = msg.riskyRightTurn; +} + +void BehaviorGen::callbackSprintSwitch(const std_msgs::Bool& msg){ + m_sprintSwitch = msg.data; +} + +void BehaviorGen::callbackGetTwistRaw(const geometry_msgs::TwistStampedConstPtr& msg) +{ + m_Twist_raw = *msg; +} + +void BehaviorGen::callbackGetTwistCMD(const geometry_msgs::TwistStampedConstPtr& msg) +{ + m_Twist_cmd = *msg; +} + +void BehaviorGen::callbackGetCommandCMD(const autoware_msgs::ControlCommandConstPtr& msg) +{ + m_Ctrl_cmd = *msg; +} + +void BehaviorGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) +{ + m_VehicleStatus.speed = msg->speed/3.6; + m_CurrentPos.v = m_VehicleStatus.speed; + m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; +} + +void BehaviorGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) +{ + m_VehicleStatus.speed = msg->twist.twist.linear.x; + m_CurrentPos.v = m_VehicleStatus.speed ; + if(msg->twist.twist.linear.x != 0) + m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; +} + +void BehaviorGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) +{ + if(msg->lanes.size() > 0 && bMap) + { + + bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); + + m_GlobalPaths.clear(); + + for(unsigned int i = 0 ; i < msg->lanes.size(); i++) + { + PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); + PlannerHNS::Lane* pPrevValid = 0; + for(unsigned int j = 0 ; j < m_temp_path.size(); j++) + { + PlannerHNS::Lane* pLane = 0; + pLane = PlannerHNS::MappingHelpers::GetLaneById(m_temp_path.at(j).laneId, m_Map); + if(!pLane) + { + pLane = PlannerHNS::MappingHelpers::GetClosestLaneFromMapDirectionBased(m_temp_path.at(j), m_Map, 1); + + if(!pLane && !pPrevValid) + { + ROS_ERROR("Map inconsistency between Global Path and Local Planer Map, Can't identify current lane."); + return; + } + + if(!pLane) + m_temp_path.at(j).pLane = pPrevValid; + else + { + m_temp_path.at(j).pLane = pLane; + pPrevValid = pLane ; + } + + m_temp_path.at(j).laneId = m_temp_path.at(j).pLane->id; + } + else + m_temp_path.at(j).pLane = pLane; + + + //std::cout << "StopLineInGlobalPath: " << m_temp_path.at(j).stopLineID << std::endl; + } + + PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); + m_GlobalPaths.push_back(m_temp_path); + + if(bOldGlobalPath) + { + bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); + } + } + + if(!bOldGlobalPath) + { + bWayGlobalPath = true; + bWayGlobalPathLogs = true; + for(unsigned int i = 0; i < m_GlobalPaths.size(); i++) + { + PlannerHNS::PlanningHelpers::FixPathDensity(m_GlobalPaths.at(i), m_PlanningParams.pathDensity); + PlannerHNS::PlanningHelpers::SmoothPath(m_GlobalPaths.at(i), 0.35, 0.4, 0.05); + + PlannerHNS::PlanningHelpers::GenerateRecommendedSpeed(m_GlobalPaths.at(i), m_CarInfo.max_speed_forward, m_PlanningParams.speedProfileFactor); + m_GlobalPaths.at(i).at(m_GlobalPaths.at(i).size()-1).v = 0; + } + + std::cout << "Received New Global Path Selector! " << std::endl; + } + else + { + m_GlobalPaths.clear(); + } + } +} + +void BehaviorGen::callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstPtr& msg) +{ + if(m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior == PlannerHNS::INTERSECTION_STATE){ + bBestCost = true; + m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance; + m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity; + return; + } + bBestCost = true; + m_TrajectoryBestCost.bBlocked = msg->is_blocked; + m_TrajectoryBestCost.index = msg->lane_index; + m_TrajectoryBestCost.cost = msg->cost; + m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance; + m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity; +} + +void BehaviorGen::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) +{ + rubis::instance_ = msg->instance; + lane_array_with_pose_twist_msg_ = msg; + + // Callback + m_VehicleStatus.speed = msg->twist.twist.linear.x; + m_CurrentPos.v = m_VehicleStatus.speed; + if(fabs(msg->twist.twist.linear.x) > 0.25) + m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; + + m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); + bNewCurrentPos = true; + + if(msg->lane_array.lanes.size() > 0) + { + m_RollOuts.clear(); + int globalPathId_roll_outs = -1; + + for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) + { + std::vector path; + PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); + m_RollOuts.push_back(path); + + if(path.size() > 0) + globalPathId_roll_outs = path.at(0).gid; + } + + if(bWayGlobalPath && m_GlobalPaths.size() > 0) + { + if(m_GlobalPaths.at(0).size() > 0) + { + int globalPathId = m_GlobalPaths.at(0).at(0).gid; + std::cout << "Before Synchronization At Behavior Selector: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; + + if(globalPathId_roll_outs == globalPathId) + { + bWayGlobalPath = false; + m_GlobalPathsToUse = m_GlobalPaths; + m_BehaviorGenerator.SetNewGlobalPath(m_GlobalPathsToUse); + std::cout << "Synchronization At Behavior Selector: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; + } + } + } + + m_BehaviorGenerator.m_RollOuts = m_RollOuts; + bRollOuts = true; + } +} + +void BehaviorGen::callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg) +{ + bNewLightSignal = true; + std::vector simulatedLights; + for(unsigned int i = 0 ; i < msg.signals.size() ; i++) + { + PlannerHNS::TrafficLight tl; + tl.id = msg.signals.at(i).id; + tl.remainTime = msg.signals.at(i).time; + + for(unsigned int k = 0; k < m_Map.trafficLights.size(); k++) + { + if(m_Map.trafficLights.at(k).id == tl.id) + { + tl.pos = m_Map.trafficLights.at(k).pos; + tl.routine = m_Map.trafficLights.at(k).routine; + break; + } + } + + if(msg.signals.at(i).type == 0) + { + tl.lightState = PlannerHNS::RED_LIGHT; + } + else if(msg.signals.at(i).type == 1) + { + tl.lightState = PlannerHNS::YELLOW_LIGHT; + } + else + { + tl.lightState = PlannerHNS::GREEN_LIGHT; + } + + simulatedLights.push_back(tl); + } + + m_CurrTrafficLight = simulatedLights; +} + +void BehaviorGen::VisualizeLocalPlanner() +{ + visualization_msgs::Marker behavior_rviz; + int iDirection = 0; + if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory > m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory) + iDirection = 1; + else if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory < m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory) + iDirection = -1; + PlannerHNS::ROSHelpers::VisualizeBehaviorState(m_CurrentPos, m_CurrentBehavior, !m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bTrafficIsRed , iDirection, behavior_rviz, "beh_state"); + //pub_BehaviorStateRviz.publish(behavior_rviz); + + visualization_msgs::MarkerArray markerArray; + + //PlannerHNS::ROSHelpers::GetIndicatorArrows(m_CurrentPos, m_CarInfo.width, m_CarInfo.length, m_CurrentBehavior.indicator, 0, markerArray); + + markerArray.markers.push_back(behavior_rviz); + + pub_BehaviorStateRviz.publish(markerArray); + + //To Test Synchronization Problem +// visualization_msgs::MarkerArray selected_path; +// std::vector > > paths; +// paths.push_back(std::vector >()); +// paths.at(0).push_back(m_BehaviorGenerator.m_Path); +// paths.push_back(m_GlobalPathsToUse); +// paths.push_back(m_RollOuts); +// PlannerHNS::ROSHelpers::TrajectoriesToMarkers(paths, selected_path); +// pub_SelectedPathRviz.publish(selected_path); +} + +void BehaviorGen::SendLocalPlanningTopics(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) +{ + //Send Behavior State + geometry_msgs::Twist t; + geometry_msgs::TwistStamped behavior; + t.linear.x = m_CurrentBehavior.bNewPlan; + t.linear.y = m_CurrentBehavior.followDistance; + t.linear.z = m_CurrentBehavior.followVelocity; + t.angular.x = (int)m_CurrentBehavior.indicator; + t.angular.y = (int)m_CurrentBehavior.state; + t.angular.z = m_CurrentBehavior.iTrajectory; + behavior.twist = t; + behavior.header.stamp = ros::Time::now(); + pub_BehaviorState.publish(behavior); + + //Send Ego Vehicle Simulation Pose Data + geometry_msgs::PoseArray sim_data; + geometry_msgs::Pose p_id, p_pose, p_box; + + sim_data.header.frame_id = "map"; + sim_data.header.stamp = ros::Time(); + p_id.position.x = 0; + p_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a)); + + PlannerHNS::WayPoint pose_center = PlannerHNS::PlanningHelpers::GetRealCenter(m_BehaviorGenerator.state, m_CarInfo.wheel_base); + + p_pose.position.x = pose_center.pos.x; + p_pose.position.y = pose_center.pos.y; + p_pose.position.z = pose_center.pos.z; + p_box.position.x = m_BehaviorGenerator.m_CarInfo.width; + p_box.position.y = m_BehaviorGenerator.m_CarInfo.length; + p_box.position.z = 2.2; + sim_data.poses.push_back(p_id); + sim_data.poses.push_back(p_pose); + sim_data.poses.push_back(p_box); + pub_SimuBoxPose.publish(sim_data); + + //Send Trajectory Data to path following nodes + std_msgs::Int32 closest_waypoint; + PlannerHNS::RelativeInfo info; + PlannerHNS::PlanningHelpers::GetRelativeInfo(m_BehaviorGenerator.m_Path, m_BehaviorGenerator.state, info); + PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_BehaviorGenerator.m_Path, m_CurrentTrajectoryToSend, info.iBack); + //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl; + + closest_waypoint.data = 1; + pub_ClosestIndex.publish(closest_waypoint); + pub_LocalBasePath.publish(m_CurrentTrajectoryToSend); + + rubis_msgs::LaneWithPoseTwist final_waypoints_with_pose_twist_msg; + final_waypoints_with_pose_twist_msg.instance = rubis::instance_; + final_waypoints_with_pose_twist_msg.lane = m_CurrentTrajectoryToSend; + final_waypoints_with_pose_twist_msg.pose = msg->pose; + final_waypoints_with_pose_twist_msg.twist = msg->twist; + + pub_LocalPathWithPosePub.publish(final_waypoints_with_pose_twist_msg); + pub_LocalPath.publish(m_CurrentTrajectoryToSend); + + +} + +void BehaviorGen::LogLocalPlanningInfo(double dt) +{ + timespec log_t; + UtilityHNS::UtilityH::GetTickCount(log_t); + std::ostringstream dataLine; + dataLine << UtilityHNS::UtilityH::GetLongTime(log_t) <<"," << dt << "," << m_CurrentBehavior.state << ","<< PlannerHNS::ROSHelpers::GetBehaviorNameFromCode(m_CurrentBehavior.state) << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->m_pParams->rollOutNumber << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bFullyBlock << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentStopSignID << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentTrafficLightID << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->minStoppingDistance << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->distanceToNext << "," << + m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->velocityOfNext << "," << + m_CurrentBehavior.maxVelocity << "," << + m_Twist_raw.twist.linear.x << "," << + m_Twist_cmd.twist.linear.x << "," << + m_Ctrl_cmd.linear_velocity << "," << + m_VehicleStatus.speed << "," << + m_VehicleStatus.steer << "," << + m_BehaviorGenerator.state.pos.x << "," << m_BehaviorGenerator.state.pos.y << "," << m_BehaviorGenerator.state.pos.z << "," << UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a)+M_PI << ","; + m_LogData.push_back(dataLine.str()); + + + if(bWayGlobalPathLogs) + { + for(unsigned int i=0; i < m_GlobalPaths.size(); i++) + { + std::ostringstream str_out; + str_out << UtilityHNS::UtilityH::GetHomeDirectory(); + str_out << UtilityHNS::DataRW::LoggingMainfolderName; + str_out << UtilityHNS::DataRW::PathLogFolderName; + str_out << "Global_Vel"; + str_out << i; + str_out << "_"; + PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(), m_GlobalPaths.at(i)); + } + bWayGlobalPathLogs = false; + } +} + +void BehaviorGen::CalculateTurnAngle(PlannerHNS::WayPoint turn_point){ + geometry_msgs::PoseStamped turn_pose; + + if(GetBaseMapTF()){ + // std::cout<<"BEFORE:"<("/op_behavior_selector/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv"); + private_nh.param("/op_behavior_selector/rate", rate, 10); + private_nh.param("/op_behavior_selector/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + private_nh.param("/op_behavior_selector/task_execution_time", task_execution_time, (double)10); + private_nh.param("/op_behavior_selector/task_relative_deadline", task_relative_deadline, (double)10); + + /* For Task scheduling */ + rubis::init_task_profiling(task_response_time_filename); + + m_sprintSwitch = false; + + ros::Rate r(100); + while(ros::ok()){ + rubis::start_task_profiling(); + + ros::spinOnce(); + + // Check Pedestrian is Appeared + double dt = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer); + UtilityHNS::UtilityH::GetTickCount(planningTimer); + + if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) + { + bMap = true; + PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); + } + else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) + { + bMap = true; + PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); + + } + else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) + { + std::vector conn_data;; + + if(m_MapRaw.GetVersion()==2) + { + PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, + m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, + m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, + m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, + m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, + m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); + + try{ + // Add Traffic Signal Info from yaml file + XmlRpc::XmlRpcValue traffic_light_list; + nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list); + + // Add Stop Line Info from yaml file + XmlRpc::XmlRpcValue stop_line_list; + nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list); + + // Add Crossing Info from yaml file + // XmlRpc::XmlRpcValue intersection_list; + // nh.getParam("/op_behavior_selector/intersection_list", intersection_list); + + PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list); + } + catch(XmlRpc::XmlRpcException& e){ + ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str()); + exit(1); + } + + m_BehaviorGenerator.m_Map = m_Map; + + if(m_Map.roadSegments.size() > 0) + { + bMap = true; + std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl; + } + } + else if(m_MapRaw.GetVersion()==1) + { + PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, + m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, + m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, + m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, + m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); + + if(m_Map.roadSegments.size() > 0) + { + bMap = true; + std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl; + } + } + } + + if(bNewCurrentPos && m_GlobalPaths.size()>0) + { + if(bNewLightSignal) + { + m_PrevTrafficLight = m_CurrTrafficLight; + bNewLightSignal = false; + } + + if(bNewLightStatus) + { + bNewLightStatus = false; + for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++) + m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus; + } + + m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch; + m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0); + std_msgs::Int32 curr_state_msg; + curr_state_msg.data = m_CurrentBehavior.state; + + pub_currentState.publish(curr_state_msg); + + CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint); + m_BehaviorGenerator.m_turnAngle = m_turnAngle; + + std_msgs::Float64 turn_angle_msg; + turn_angle_msg.data = m_turnAngle; + pub_turnAngle.publish(turn_angle_msg); + + emergency_stop_msg.data = false; + if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop! + emergency_stop_msg.data = true; + pub_EmergencyStop.publish(emergency_stop_msg); + + SendLocalPlanningTopics(lane_array_with_pose_twist_msg_); + VisualizeLocalPlanner(); + LogLocalPlanningInfo(dt); + + // Publish turn_marker + visualization_msgs::MarkerArray turn_marker; + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.type = 2; + marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x; + marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y; + marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z; + marker.scale.x = 3; + marker.scale.y = 3; + marker.scale.z = 3; + marker.color.r = 0.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + marker.color.a = 1.0f; + marker.header.stamp = ros::Time::now(); + marker.header.frame_id = "map"; + turn_marker.markers.push_back(marker); + + pub_turnMarker.publish(turn_marker); + } + else + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); + + rubis::stop_task_profiling(0, 0); + r.sleep(); + } +} + +bool BehaviorGen::GetBaseMapTF(){ + + try{ + m_map_base_listener.waitForTransform("base_link", "map", ros::Time(0), ros::Duration(0.001)); + m_map_base_listener.lookupTransform("base_link", "map", ros::Time(0), m_map_base_transform); + return true; + } + catch(tf::TransformException& ex) + { + return false; + } + +} + +void BehaviorGen::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped& out_pose, const tf::StampedTransform &in_transform) +{ + + tf::Vector3 in_pos(in_pose.pose.position.x, + in_pose.pose.position.y, + in_pose.pose.position.z); + tf::Quaternion in_quat(in_pose.pose.orientation.x, + in_pose.pose.orientation.y, + in_pose.pose.orientation.w, + in_pose.pose.orientation.z); + + tf::Vector3 in_pos_t = in_transform * in_pos; + tf::Quaternion in_quat_t = in_transform * in_quat; + + out_pose.header = in_pose.header; + out_pose.pose.position.x = in_pos_t.x(); + out_pose.pose.position.y = in_pos_t.y(); + out_pose.pose.position.z = in_pos_t.z(); + out_pose.pose.orientation.x = in_quat_t.x(); + out_pose.pose.orientation.y = in_quat_t.y(); + out_pose.pose.orientation.z = in_quat_t.z(); + + return; +} + +//Mapping Section + +void BehaviorGen::callbackGetVMLanes(const vector_map_msgs::LaneArray& msg) +{ + std::cout << "Received Lanes" << endl; + if(m_MapRaw.pLanes == nullptr) + m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg); +} + +void BehaviorGen::callbackGetVMPoints(const vector_map_msgs::PointArray& msg) +{ + std::cout << "Received Points" << endl; + if(m_MapRaw.pPoints == nullptr) + m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg); +} + +void BehaviorGen::callbackGetVMdtLanes(const vector_map_msgs::DTLaneArray& msg) +{ + std::cout << "Received dtLanes" << endl; + if(m_MapRaw.pCenterLines == nullptr) + m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg); +} + +void BehaviorGen::callbackGetVMIntersections(const vector_map_msgs::CrossRoadArray& msg) +{ + std::cout << "Received CrossRoads" << endl; + if(m_MapRaw.pIntersections == nullptr) + m_MapRaw.pIntersections = new UtilityHNS::AisanIntersectionFileReader(msg); +} + +void BehaviorGen::callbackGetVMAreas(const vector_map_msgs::AreaArray& msg) +{ + std::cout << "Received Areas" << endl; + if(m_MapRaw.pAreas == nullptr) + m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg); +} + +void BehaviorGen::callbackGetVMLines(const vector_map_msgs::LineArray& msg) +{ + std::cout << "Received Lines" << endl; + if(m_MapRaw.pLines == nullptr) + m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg); +} + +void BehaviorGen::callbackGetVMStopLines(const vector_map_msgs::StopLineArray& msg) +{ + std::cout << "Received StopLines" << endl; + if(m_MapRaw.pStopLines == nullptr) + m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg); +} + +void BehaviorGen::callbackGetVMSignal(const vector_map_msgs::SignalArray& msg) +{ + std::cout << "Received Signals" << endl; + if(m_MapRaw.pSignals == nullptr) + m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg); +} + +void BehaviorGen::callbackGetVMVectors(const vector_map_msgs::VectorArray& msg) +{ + std::cout << "Received Vectors" << endl; + if(m_MapRaw.pVectors == nullptr) + m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg); +} + +void BehaviorGen::callbackGetVMCurbs(const vector_map_msgs::CurbArray& msg) +{ + std::cout << "Received Curbs" << endl; + if(m_MapRaw.pCurbs == nullptr) + m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg); +} + +void BehaviorGen::callbackGetVMRoadEdges(const vector_map_msgs::RoadEdgeArray& msg) +{ + std::cout << "Received Edges" << endl; + if(m_MapRaw.pRoadedges == nullptr) + m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg); +} + +void BehaviorGen::callbackGetVMWayAreas(const vector_map_msgs::WayAreaArray& msg) +{ + std::cout << "Received Wayareas" << endl; + if(m_MapRaw.pWayAreas == nullptr) + m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg); +} + +void BehaviorGen::callbackGetVMCrossWalks(const vector_map_msgs::CrossWalkArray& msg) +{ + std::cout << "Received CrossWalks" << endl; + if(m_MapRaw.pCrossWalks == nullptr) + m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg); +} + +void BehaviorGen::callbackGetVMNodes(const vector_map_msgs::NodeArray& msg) +{ + std::cout << "Received Nodes" << endl; + if(m_MapRaw.pNodes == nullptr) + m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg); +} +} diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp index 8488fd1d..d31d230c 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp @@ -42,7 +42,8 @@ MotionPrediction::MotionPrediction() m_OriginPos.position.y = transform.getOrigin().y(); m_OriginPos.position.z = transform.getOrigin().z(); - pub_predicted_objects_trajectories = nh.advertise("/predicted_objects", 1); + // pub_predicted_objects_trajectories = nh.advertise("/predicted_objects", 1); + pub_rubis_predicted_objects_trajectories = nh.advertise("/rubis_predicted_objects", 1); pub_PredictedTrajectoriesRviz = nh.advertise("/predicted_trajectories_rviz", 1); pub_CurbsRviz = nh.advertise("/map_curbs_rviz", 1); pub_ParticlesRviz = nh.advertise("prediction_particles", 1); @@ -61,11 +62,17 @@ MotionPrediction::MotionPrediction() _nh.param("/op_motion_predictor/input_object_list", input_object_list_str, "[/tracked_objects]"); std::vector input_object_list = ParseInputStr(input_object_list_str); - - for(auto it = input_object_list.begin(); it != input_object_list.end(); ++it){ + for(auto it = input_object_list.begin(); it != input_object_list.end(); ++it){ std::string topic = *it; - objects_subs_.push_back(nh.subscribe(topic.c_str(), 1, &MotionPrediction::callbackGetTrackedObjects, this)); - autoware_msgs::DetectedObjectArray msg; + std::cout<instance; + + UtilityHNS::UtilityH::GetTickCount(m_SensingTimer); + m_TrackedObjects.clear(); + bTrackedObjects = true; + + // Check frame id of the object is valid + std::string target_frame = in_msg->object_array.header.frame_id; + int obj_idx = getIndex(tf_str_list_, target_frame); + if(obj_idx == -1){ + std::cout<object_array, transform_list_[obj_idx]); + } + else{ + msg = in_msg->object_array; + } + + + PlannerHNS::DetectedObject obj; + + for(unsigned int i = 0 ; i objects.at(i).id > 0) + // { + PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg.objects.at(i), obj); + m_TrackedObjects.push_back(obj); + // } + } + + if(bMap) + { + if(m_PredictBeh.m_bStepByStep && m_bGoNextStep) + { + m_bGoNextStep = false; + m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration, m_Map); + } + else if(!m_PredictBeh.m_bStepByStep) + { + m_PredictBeh.DoOneStep(m_TrackedObjects, m_CurrentPos, m_PlanningParams.minSpeed, m_CarInfo.max_deceleration, m_Map); + } + object_msg_list_[obj_idx].objects.clear(); + autoware_msgs::DetectedObject pred_obj; + for(unsigned int i = 0 ; i obj, false, pred_obj); + if(m_PredictBeh.m_ParticleInfo_II.at(i)->best_beh_track) + pred_obj.behavior_state = m_PredictBeh.m_ParticleInfo_II.at(i)->best_beh_track->best_beh; + // pred_obj = TransformObjToVeldoyne(pred_obj, transform_list_[obj_idx]); + object_msg_list_[obj_idx].objects.push_back(pred_obj); + } + + if(m_bEnableCurbObstacles) + { + curr_curbs_obstacles.clear(); + GenerateCurbsObstacles(curr_curbs_obstacles); + //std::cout << "Curbs No: " << curr_curbs_obstacles.size() << endl; + for(unsigned int i = 0 ; i & curb_obstacles) @@ -636,7 +727,7 @@ void MotionPrediction::MainLoop() UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer); } - if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); + if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp index 466b858e..9c914671 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp @@ -76,7 +76,8 @@ TrajectoryEval::TrajectoryEval() sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); sub_LocalPlannerPaths = nh.subscribe("/local_trajectories", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this); - sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); + // sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); + sub_rubis_predicted_objects = nh.subscribe("/rubis_predicted_objects", 1, &TrajectoryEval::callbackGetRubisPredictedObjects, this); sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this); PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy); @@ -260,6 +261,7 @@ void TrajectoryEval::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayC void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg) { + /* m_PredictedObjects.clear(); bPredictedObjects = true; double distance_to_pedestrian = 1000; @@ -286,33 +288,12 @@ void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedOb if(msg_obj.id > 0) // If fusion object is detected { - // calculate distance to person first - // if(msg_obj.label == "person"){ - // std::cout<<"Pedestrian box size(width x height):"< m_PedestrianLeftThreshold || temp_y_distance < m_PedestrianRightThreshold ) continue; - // if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; - // } - // catch(tf::TransformException& ex){ - // // ROS_ERROR("Cannot transform person pose: %s", ex.what()); - - // } - // } - if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ vehicle_cnt += 1; } PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj); - // transform center pose into map frame geometry_msgs::PoseStamped pose_in_map; pose_in_map.header = msg_obj.header; @@ -358,31 +339,6 @@ void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedOb m_PredictedObjects.push_back(obj); } - /* - else{ // If object is only detected at vision - int image_obj_center_x = msg_obj.x+msg_obj.width/2; - int image_obj_center_y = msg_obj.y+msg_obj.height/2; - // if (msg_obj.label == "person"){// If person is detected only in image - // // TO ERASE - // std::cout<<"object height:" << msg_obj.height << " / threshold:" << m_pedestrian_stop_img_height_threshold << std::endl; - // if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ - // double temp_x_distance = 1000; - // if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; - // if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; - // } - // } - // else - if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ - if((msg_obj.width > m_VehicleImageWidthThreshold) - && (image_obj_center_x > image_vehicle_detection_range_left) - && (image_obj_center_x < image_vehicle_detection_range_right) - ) - { - vehicle_cnt+=1; - } - } - } - */ int image_obj_center_x = msg_obj.x+msg_obj.width/2; int image_obj_center_y = msg_obj.y+msg_obj.height/2; @@ -397,7 +353,6 @@ void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedOb if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; } } - } // Publish Sprint Switch @@ -416,8 +371,122 @@ void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedOb } pub_SprintSwitch.publish(sprint_switch_msg); - // ROS_INFO("object # : %d", m_PredictedObjects.size()); + std_msgs::Float64 distanceToPedestrianMsg; + distanceToPedestrianMsg.data = distance_to_pedestrian; + pub_DistanceToPedestrian.publish(distanceToPedestrianMsg); + */ +} + +void TrajectoryEval::callbackGetRubisPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg) +{ + m_PredictedObjects.clear(); + bPredictedObjects = true; + double distance_to_pedestrian = 1000; + int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2; + int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2; + int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2; + int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2; + + int vehicle_cnt = 0; + + PlannerHNS::DetectedObject obj; + + for(unsigned int i = 0 ; i object_array.objects.size(); i++) + { + if(msg->object_array.objects.at(i).pose.position.y < -20 || msg->object_array.objects.at(i).pose.position.y > 20) + continue; + + if(msg->object_array.objects.at(i).pose.position.z > 1 || msg->object_array.objects.at(i).pose.position.z < -1.5) + continue; + + autoware_msgs::DetectedObject msg_obj = msg->object_array.objects.at(i); + + if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ + vehicle_cnt += 1; + } + + PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->object_array.objects.at(i), obj); + + geometry_msgs::PoseStamped pose_in_map; + pose_in_map.header = msg_obj.header; + pose_in_map.pose = msg_obj.pose; + while(1){ + try{ + m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map); + break; + } + catch(tf::TransformException& ex) + { + // ROS_ERROR("Cannot transform object pose: %s", ex.what()); + continue; + } + } + // msg_obj.header.frame_id = "map"; + obj.center.pos.x = pose_in_map.pose.position.x; + obj.center.pos.y = pose_in_map.pose.position.y; + obj.center.pos.z = pose_in_map.pose.position.z; + + // transform contour into map frame + for(unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++){ + geometry_msgs::PoseStamped contour_point_in_map; + contour_point_in_map.header = msg_obj.header; + contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x; + contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y; + contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z; + + // For resolve TF malform, set orientation w to 1 + contour_point_in_map.pose.orientation.w = 1; + + for(int i = 0; i < 1000; i++){ + try{ + m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map); + break; + } + catch(tf::TransformException& ex){ + // ROS_ERROR("Cannot transform contour pose: %s", ex.what()); + continue; + } + } + + obj.contour.at(j).x = contour_point_in_map.pose.position.x; + obj.contour.at(j).y = contour_point_in_map.pose.position.y; + obj.contour.at(j).z = contour_point_in_map.pose.position.z; + } + + msg_obj.header.frame_id = "map"; + + m_PredictedObjects.push_back(obj); + + int image_obj_center_x = msg_obj.x+msg_obj.width/2; + int image_obj_center_y = msg_obj.y+msg_obj.height/2; + if (msg_obj.label == "person"){// If person is detected only in image + + if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ + double temp_x_distance = 1000; + if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; + if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; + } + } + + } + + // Publish Sprint Switch + std_msgs::Bool sprint_switch_msg; + + if(vehicle_cnt != 0){ + m_noVehicleCnt = 0; + sprint_switch_msg.data = false; + } + else{ // No vehicle is exist in front of the car + if(m_noVehicleCnt < m_SprintDecisionTime*10) { + m_noVehicleCnt +=1; + sprint_switch_msg.data = false; + } + else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true; + } + pub_SprintSwitch.publish(sprint_switch_msg); + std_msgs::Float64 distanceToPedestrianMsg; distanceToPedestrianMsg.data = distance_to_pedestrian; pub_DistanceToPedestrian.publish(distanceToPedestrianMsg); @@ -441,6 +510,11 @@ void TrajectoryEval::UpdateMyParams() _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong); _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat); _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance); + + _nh.getParam("/op_trajectory_evaluator/lateralBlockingThreshold", m_PlanningParams.lateralBlockingThreshold); + _nh.getParam("/op_trajectory_evaluator/frontLongitudinalBlockingThreshold", m_PlanningParams.frontLongitudinalBlockingThreshold); + _nh.getParam("/op_trajectory_evaluator/rearLongitudinalBlockingThreshold", m_PlanningParams.rearLongitudinalBlockingThreshold); + _nh.getParam("/op_trajectory_evaluator/enableDebug", m_PlanningParams.enableDebug); } bool TrajectoryEval::UpdateTf() @@ -510,7 +584,6 @@ void TrajectoryEval::MainLoop() UpdateMyParams(); UpdateTf(); - ros::spinOnce(); PlannerHNS::TrajectoryCost tc; diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp new file mode 100644 index 00000000..a1ce5e83 --- /dev/null +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp @@ -0,0 +1,596 @@ +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "op_trajectory_evaluator_core.h" +#include "op_ros_helpers/op_ROSHelpers.h" +#include "op_planner/MappingHelpers.h" +#include + +namespace TrajectoryEvaluatorNS +{ + +TrajectoryEval::TrajectoryEval() +{ + bNewCurrentPos = false; + bVehicleStatus = false; + bWayGlobalPath = false; + bWayGlobalPathToUse = false; + m_bUseMoveingObjectsPrediction = false; + m_noVehicleCnt = 0; + + ros::NodeHandle _nh; + UpdatePlanningParams(_nh); + + tf::StampedTransform transform; + PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); + m_OriginPos.position.x = transform.getOrigin().x(); + m_OriginPos.position.y = transform.getOrigin().y(); + m_OriginPos.position.z = transform.getOrigin().z(); + + pub_CollisionPointsRviz = nh.advertise("dynamic_collision_points_rviz", 1); + pub_LocalWeightedTrajectoriesRviz = nh.advertise("local_trajectories_eval_rviz", 1); + pub_LocalWeightedTrajectories = nh.advertise("local_weighted_trajectories", 1); + pub_LocalWeightedTrajectoriesWithPoseTwist = nh.advertise("local_weighted_trajectories_with_pose_twist", 1); + pub_TrajectoryCost = nh.advertise("local_trajectory_cost", 1); + pub_SafetyBorderRviz = nh.advertise("safety_border", 1); + pub_DistanceToPedestrian = nh.advertise("distance_to_pedestrian", 1); + pub_IntersectionCondition = nh.advertise("intersection_condition", 1); + pub_SprintSwitch = nh.advertise("sprint_switch", 1); + + // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this); + // sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this); + + int bVelSource = 1; + _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); + if(bVelSource == 0) + sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this); + // else if(bVelSource == 1) + // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this); + else if(bVelSource == 2) + sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this); + + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); + sub_LocalPlannerPaths = nh.subscribe("/local_trajectories_with_pose_twist", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this); + sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); + sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this); + + PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy); + + while(1){ + if(UpdateTf() == true) + break; + } +} + +TrajectoryEval::~TrajectoryEval() +{ +} + +void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh) +{ + _nh.getParam("/op_trajectory_evaluator/enablePrediction", m_bUseMoveingObjectsPrediction); + + _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); + _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); + _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); + if(m_PlanningParams.enableSwerving) + m_PlanningParams.enableFollowing = true; + else + _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); + + _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); + _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); + + _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); + _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); + _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); + + _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); + + _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); + if(m_PlanningParams.enableSwerving) + _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); + else + m_PlanningParams.rollOutNumber = 0; + + std::cout << "Rolls Number: " << m_PlanningParams.rollOutNumber << std::endl; + + _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); + _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); + _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); + _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); + _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); + + _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); + + _nh.getParam("/op_common_params/width", m_CarInfo.width); + _nh.getParam("/op_common_params/length", m_CarInfo.length); + _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); + _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); + _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); + _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); + _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); + m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; + m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; + + _nh.param("/op_trajectory_evaluator/PedestrianRightThreshold", m_PedestrianRightThreshold, 7.0); + _nh.param("/op_trajectory_evaluator/PedestrianLeftThreshold", m_PedestrianLeftThreshold, 2.0); + _nh.param("/op_trajectory_evaluator/PedestrianImageDetectionRange", m_PedestrianImageDetectionRange, 0.7); + _nh.param("/op_trajectory_evaluator/PedestrianStopImgHeightThreshold", m_pedestrian_stop_img_height_threshold, 120); + _nh.param("/op_trajectory_evaluator/ImageWidth", m_ImageWidth, 1920); + _nh.param("/op_trajectory_evaluator/ImageHeight", m_ImageHeight, 1080); + _nh.param("/op_trajectory_evaluator/VehicleImageDetectionRange", m_VehicleImageDetectionRange, 0.3); + _nh.param("/op_trajectory_evaluator/VehicleImageWidthThreshold", m_VehicleImageWidthThreshold, 0.05); + _nh.param("/op_trajectory_evaluator/SprintDecisionTime", m_SprintDecisionTime, 5.0); + + + m_VehicleImageWidthThreshold = m_VehicleImageWidthThreshold * m_ImageWidth; + m_PedestrianRightThreshold *= -1; + +} + +// void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +// { +// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); +// bNewCurrentPos = true; +// } + +// void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) +// { +// m_VehicleStatus.speed = msg->twist.linear.x; +// m_CurrentPos.v = m_VehicleStatus.speed; +// if(fabs(msg->twist.linear.x) > 0.25) +// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); +// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); +// bVehicleStatus = true; +// } + +void TrajectoryEval::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) +{ + m_VehicleStatus.speed = msg->speed/3.6; + m_CurrentPos.v = m_VehicleStatus.speed; + m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; +} + +void TrajectoryEval::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) +{ + m_VehicleStatus.speed = msg->twist.twist.linear.x; + m_CurrentPos.v = m_VehicleStatus.speed; + if(fabs(msg->twist.twist.linear.x) > 0.25) + m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; +} + +void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) +{ + if(msg->lanes.size() > 0) + { + + bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); + + m_GlobalPaths.clear(); + + for(unsigned int i = 0 ; i < msg->lanes.size(); i++) + { + PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); + + PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); + m_GlobalPaths.push_back(m_temp_path); + + if(bOldGlobalPath) + { + bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); + } + } + + if(!bOldGlobalPath) + { + bWayGlobalPath = true; + std::cout << "Received New Global Path Evaluator! " << std::endl; + } + else + { + m_GlobalPaths.clear(); + } + } +} + +void TrajectoryEval::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) +{ + rubis::instance_ = msg->instance; + // Callback + m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); + bNewCurrentPos = true; + + m_VehicleStatus.speed = msg->twist.twist.linear.x; + m_CurrentPos.v = m_VehicleStatus.speed; + if(fabs(msg->twist.twist.linear.x) > 0.25) + m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; + + if(msg->lane_array.lanes.size() > 0) + { + m_GeneratedRollOuts.clear(); + int globalPathId_roll_outs = -1; + + for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) + { + std::vector path; + PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); + m_GeneratedRollOuts.push_back(path); + if(path.size() > 0) + globalPathId_roll_outs = path.at(0).gid; + } + + if(bWayGlobalPath && m_GlobalPaths.size() > 0 && m_GlobalPaths.at(0).size() > 0) + { + int globalPathId = m_GlobalPaths.at(0).at(0).gid; + std::cout << "Before Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; + + if(globalPathId_roll_outs == globalPathId) + { + bWayGlobalPath = false; + m_GlobalPathsToUse = m_GlobalPaths; + std::cout << "Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; + } + } + + bRollOuts = true; + } +} + +void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg) +{ + m_PredictedObjects.clear(); + bPredictedObjects = true; + double distance_to_pedestrian = 1000; + int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2; + int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2; + + int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2; + int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2; + + int vehicle_cnt = 0; + + PlannerHNS::DetectedObject obj; + for(unsigned int i = 0 ; i objects.size(); i++) + { + if(msg->objects.at(i).pose.position.y < -20 || msg->objects.at(i).pose.position.y > 20) + continue; + + if(msg->objects.at(i).pose.position.z > 1 || msg->objects.at(i).pose.position.z < -1.5) + continue; + + autoware_msgs::DetectedObject msg_obj = msg->objects.at(i); + + // #### Decison making for objects + + if(msg_obj.id > 0) // If fusion object is detected + { + // calculate distance to person first + // if(msg_obj.label == "person"){ + // std::cout<<"Pedestrian box size(width x height):"< m_PedestrianLeftThreshold || temp_y_distance < m_PedestrianRightThreshold ) continue; + // if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; + // } + // catch(tf::TransformException& ex){ + // // ROS_ERROR("Cannot transform person pose: %s", ex.what()); + + // } + // } + + if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ + vehicle_cnt += 1; + } + + PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj); + + + // transform center pose into map frame + geometry_msgs::PoseStamped pose_in_map; + pose_in_map.header = msg_obj.header; + pose_in_map.pose = msg_obj.pose; + try{ + m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map); + } + catch(tf::TransformException& ex) + { + // ROS_ERROR("Cannot transform object pose: %s", ex.what()); + continue; + } + // msg_obj.header.frame_id = "map"; + obj.center.pos.x = pose_in_map.pose.position.x; + obj.center.pos.y = pose_in_map.pose.position.y; + obj.center.pos.z = pose_in_map.pose.position.z; + + // transform contour into map frame + for(unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++){ + geometry_msgs::PoseStamped contour_point_in_map; + contour_point_in_map.header = msg_obj.header; + contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x; + contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y; + contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z; + + // For resolve TF malform, set orientation w to 1 + contour_point_in_map.pose.orientation.w = 1; + + try{ + m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map); + } + catch(tf::TransformException& ex){ + // ROS_ERROR("Cannot transform contour pose: %s", ex.what()); + continue; + } + + obj.contour.at(j).x = contour_point_in_map.pose.position.x; + obj.contour.at(j).y = contour_point_in_map.pose.position.y; + obj.contour.at(j).z = contour_point_in_map.pose.position.z; + } + + msg_obj.header.frame_id = "map"; + + m_PredictedObjects.push_back(obj); + } + /* + else{ // If object is only detected at vision + int image_obj_center_x = msg_obj.x+msg_obj.width/2; + int image_obj_center_y = msg_obj.y+msg_obj.height/2; + // if (msg_obj.label == "person"){// If person is detected only in image + // // TO ERASE + // std::cout<<"object height:" << msg_obj.height << " / threshold:" << m_pedestrian_stop_img_height_threshold << std::endl; + // if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ + // double temp_x_distance = 1000; + // if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; + // if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; + // } + // } + // else + if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ + if((msg_obj.width > m_VehicleImageWidthThreshold) + && (image_obj_center_x > image_vehicle_detection_range_left) + && (image_obj_center_x < image_vehicle_detection_range_right) + ) + { + vehicle_cnt+=1; + } + } + } + */ + + int image_obj_center_x = msg_obj.x+msg_obj.width/2; + int image_obj_center_y = msg_obj.y+msg_obj.height/2; + if (msg_obj.label == "person"){// If person is detected only in image + + // TO ERASE + // ROS_WARN("object height:%d // thr: %d\n", msg_obj.height, m_pedestrian_stop_img_height_threshold); + printf("center_x %d \n left: %d \n right %d\n\n\n", image_obj_center_x, image_person_detection_range_left, image_person_detection_range_right); + if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ + double temp_x_distance = 1000; + if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; + if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; + } + } + + } + + // Publish Sprint Switch + std_msgs::Bool sprint_switch_msg; + + if(vehicle_cnt != 0){ + m_noVehicleCnt = 0; + sprint_switch_msg.data = false; + } + else{ // No vehicle is exist in front of the car + if(m_noVehicleCnt < m_SprintDecisionTime*10) { + m_noVehicleCnt +=1; + sprint_switch_msg.data = false; + } + else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true; + } + pub_SprintSwitch.publish(sprint_switch_msg); + + // ROS_INFO("object # : %d", m_PredictedObjects.size()); + + std_msgs::Float64 distanceToPedestrianMsg; + distanceToPedestrianMsg.data = distance_to_pedestrian; + pub_DistanceToPedestrian.publish(distanceToPedestrianMsg); +} + +void TrajectoryEval::callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr& msg) +{ + m_CurrentBehavior.iTrajectory = msg->twist.angular.z; +} + +void TrajectoryEval::callbackGetCurrentState(const std_msgs::Int32 & msg) +{ + m_CurrentBehavior.state = static_cast(msg.data); +} + +void TrajectoryEval::UpdateMyParams() +{ + ros::NodeHandle _nh; + _nh.getParam("/op_trajectory_evaluator/weightPriority", m_PlanningParams.weightPriority); + _nh.getParam("/op_trajectory_evaluator/weightTransition", m_PlanningParams.weightTransition); + _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong); + _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat); + _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance); +} + +bool TrajectoryEval::UpdateTf() +{ + try{ + m_vtob_listener.waitForTransform("/velodyne", "/base_link", ros::Time(0), ros::Duration(0.001)); + m_vtob_listener.lookupTransform("/velodyne", "/base_link", ros::Time(0), m_velodyne_to_base_link); + + m_vtom_listener.waitForTransform("/velodyne", "/map", ros::Time(0), ros::Duration(0.001)); + m_vtom_listener.lookupTransform("/velodyne", "/map", ros::Time(0), m_velodyne_to_map); + return true; + } + catch(tf::TransformException& ex){ + if(TF_DEBUG) + ROS_ERROR("%s", ex.what()); + return false; + } +} + +void TrajectoryEval::MainLoop() +{ + ros::NodeHandle private_nh("~"); + + // Scheduling Setup + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; + double task_execution_time; + double task_relative_deadline; + + private_nh.param("/op_trajectory_evaluator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"); + private_nh.param("/op_trajectory_evaluator/rate", rate, 10); + private_nh.param("/op_trajectory_evaluator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + private_nh.param("/op_trajectory_evaluator/task_execution_time", task_execution_time, (double)10); + private_nh.param("/op_trajectory_evaluator/task_relative_deadline", task_relative_deadline, (double)10); + + rubis::init_task_profiling(task_response_time_filename); + + PlannerHNS::WayPoint prevState, state_change; + + // Add Crossing Info from yaml file + XmlRpc::XmlRpcValue intersection_xml; + nh.getParam("/op_trajectory_evaluator/intersection_list", intersection_xml); + PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list_, intersection_xml); + + ros::Rate r(100); + + while(ros::ok()){ + // Before spin + UpdateMyParams(); + UpdateTf(); + + ros::spinOnce(); + + PlannerHNS::TrajectoryCost tc; + + if(bNewCurrentPos && m_GlobalPaths.size()>0) + { + m_GlobalPathSections.clear(); + + for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++) + { + t_centerTrajectorySmoothed.clear(); + PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); + m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); + } + + if(m_GlobalPathSections.size()>0) + { + if(m_bUseMoveingObjectsPrediction) + tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory); + else + tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state); + + autoware_msgs::Lane l; + l.closest_object_distance = tc.closest_obj_distance; + l.closest_object_velocity = tc.closest_obj_velocity; + l.cost = tc.cost; + l.is_blocked = tc.bBlocked; + l.lane_index = tc.index; + pub_TrajectoryCost.publish(l); + + // hjw added : Check if ego is on intersection and obstacles are in risky area + int intersectionID = -1; + double closestIntersectionDistance = -1; + bool isInsideIntersection = false; + bool riskyLeftTurn = false; + bool riskyRightTurn = false; + + PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list_, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn); + + autoware_msgs::IntersectionCondition ic_msg; + ic_msg.intersectionID = intersectionID; + ic_msg.intersectionDistance = closestIntersectionDistance; + ic_msg.isIntersection = isInsideIntersection; + ic_msg.riskyLeftTurn = riskyLeftTurn; + ic_msg.riskyRightTurn = riskyRightTurn; + + pub_IntersectionCondition.publish(ic_msg); + + } + + if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) + { + rubis_msgs::LaneArrayWithPoseTwist local_lanes; + for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++) + { + autoware_msgs::Lane lane; + PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane); + lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance; + lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity; + lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost; + lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked; + lane.lane_index = i; + local_lanes.lane_array.lanes.push_back(lane); + } + + local_lanes.instance = rubis::instance_; + local_lanes.pose = msg->pose; + local_lanes.twist = msg->twist; + + pub_LocalWeightedTrajectoriesWithPoseTwist.publish(local_lanes); + pub_LocalWeightedTrajectories.publish(local_lanes.lane_array); + + } + else + { + ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()"); + } + + if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0) + { + visualization_msgs::MarkerArray all_rollOuts; + PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts); + pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts); + + PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy); + pub_CollisionPointsRviz.publish(m_CollisionsActual); + + //Visualize Safety Box + visualization_msgs::Marker safety_box; + PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box); + pub_SafetyBorderRviz.publish(safety_box); + } + } + else + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); + + rubis::stop_task_profiling(0, 0); + + r.sleep(); + } +} + +} diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp new file mode 100644 index 00000000..2c40bb81 --- /dev/null +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp @@ -0,0 +1,315 @@ +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "op_trajectory_generator_core.h" +#include "op_ros_helpers/op_ROSHelpers.h" +#include + +#define SPIN_PROFILING + +namespace TrajectoryGeneratorNS +{ + +TrajectoryGen::TrajectoryGen() +{ + bInitPos = false; + bNewCurrentPos = false; + bVehicleStatus = false; + bWayGlobalPath = false; + + ros::NodeHandle _nh; + UpdatePlanningParams(_nh); + + tf::StampedTransform transform; + PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); + m_OriginPos.position.x = transform.getOrigin().x(); + m_OriginPos.position.y = transform.getOrigin().y(); + m_OriginPos.position.z = transform.getOrigin().z(); + + pub_LocalTrajectories = nh.advertise("local_trajectories", 1); + pub_LocalTrajectoriesWithPoseTwist = nh.advertise("local_trajectories_with_pose_twist", 1); + pub_LocalTrajectoriesRviz = nh.advertise("local_trajectories_gen_rviz", 1); + + sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this); + // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryGen::callbackGetCurrentPose, this); + + int bVelSource = 1; + _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource); + if(bVelSource == 0) + sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryGen::callbackGetRobotOdom, this); + // else if(bVelSource == 1) + // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this); + else if(bVelSource == 2) + sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this); + + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); + + sub_pose_twist = nh.subscribe("/rubis_current_pose_twist", 10, &TrajectoryGen::callbackGetCurrentPoseTwist, this); +} + +TrajectoryGen::~TrajectoryGen() +{ +} + +void TrajectoryGen::UpdatePlanningParams(ros::NodeHandle& _nh) +{ + _nh.getParam("/op_trajectory_generator/samplingTipMargin", m_PlanningParams.carTipMargin); + _nh.getParam("/op_trajectory_generator/samplingOutMargin", m_PlanningParams.rollInMargin); + _nh.getParam("/op_trajectory_generator/samplingSpeedFactor", m_PlanningParams.rollInSpeedFactor); + _nh.getParam("/op_trajectory_generator/enableHeadingSmoothing", m_PlanningParams.enableHeadingSmoothing); + + _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); + if(m_PlanningParams.enableSwerving) + m_PlanningParams.enableFollowing = true; + else + _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); + + _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); + _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); + + _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); + _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); + _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); + + _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); + _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); + if(m_PlanningParams.enableSwerving) + _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); + else + m_PlanningParams.rollOutNumber = 0; + + _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); + _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); + _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); + _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); + _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); + + _nh.getParam("/op_common_params/smoothingDataWeight", m_PlanningParams.smoothingDataWeight); + _nh.getParam("/op_common_params/smoothingSmoothWeight", m_PlanningParams.smoothingSmoothWeight); + + _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); + _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); + + _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); + + _nh.getParam("/op_common_params/width", m_CarInfo.width); + _nh.getParam("/op_common_params/length", m_CarInfo.length); + _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); + _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); + _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); + _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); + _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); + + m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; + m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; + +} + +void TrajectoryGen::callbackGetInitPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) +{ + if(!bInitPos) + { + m_InitPos = PlannerHNS::WayPoint(msg->pose.pose.position.x+m_OriginPos.position.x, + msg->pose.pose.position.y+m_OriginPos.position.y, + msg->pose.pose.position.z+m_OriginPos.position.z, + tf::getYaw(msg->pose.pose.orientation)); + m_CurrentPos = m_InitPos; + bInitPos = true; + } +} + +// void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +// { +// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); +// m_InitPos = m_CurrentPos; +// bNewCurrentPos = true; +// bInitPos = true; +// } + +// void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) +// { +// m_VehicleStatus.speed = msg->twist.linear.x; +// m_CurrentPos.v = m_VehicleStatus.speed; +// if(fabs(msg->twist.linear.x) > 0.25) +// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); +// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); +// bVehicleStatus = true; +// } + +void TrajectoryGen::callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg){ + // Before spinOnce + rubis::start_task_profiling(); + + // Callback + m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); + m_InitPos = m_CurrentPos; + bNewCurrentPos = true; + bInitPos = true; + + m_VehicleStatus.speed = msg->twist.twist.linear.x; + m_CurrentPos.v = m_VehicleStatus.speed; + if(fabs(msg->twist.twist.linear.x) > 0.25) + m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; + + // After spinOnce + if(bInitPos && m_GlobalPaths.size()>0) + { + m_GlobalPathSections.clear(); + + for(unsigned int i = 0; i < m_GlobalPaths.size(); i++) + { + t_centerTrajectorySmoothed.clear(); + PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPaths.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , + m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); + + m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); + } + + std::vector sampledPoints_debug; + m_Planner.GenerateRunoffTrajectory(m_GlobalPathSections, m_CurrentPos, + m_PlanningParams.enableLaneChange, + m_VehicleStatus.speed, + m_PlanningParams.microPlanDistance, + m_PlanningParams.maxSpeed, + m_PlanningParams.minSpeed, + m_PlanningParams.carTipMargin, + m_PlanningParams.rollInMargin, + m_PlanningParams.rollInSpeedFactor, + m_PlanningParams.pathDensity, + m_PlanningParams.rollOutDensity, + m_PlanningParams.rollOutNumber, + m_PlanningParams.smoothingDataWeight, + m_PlanningParams.smoothingSmoothWeight, + m_PlanningParams.smoothingToleranceError, + m_PlanningParams.speedProfileFactor, + m_PlanningParams.enableHeadingSmoothing, + -1 , -1, + m_RollOuts, sampledPoints_debug); + + rubis_msgs::LaneArrayWithPoseTwist local_lanes; + for(unsigned int i=0; i < m_RollOuts.size(); i++) + { + for(unsigned int j=0; j < m_RollOuts.at(i).size(); j++) + { + autoware_msgs::Lane lane; + PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(m_RollOuts.at(i).at(j), m_CurrentPos, m_PlanningParams.minSpeed, m_PlanningParams.microPlanDistance); + PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_RollOuts.at(i).at(j), lane); + lane.closest_object_distance = 0; + lane.closest_object_velocity = 0; + lane.cost = 0; + lane.is_blocked = false; + lane.lane_index = i; + local_lanes.lane_array.lanes.push_back(lane); + } + } + + rubis::instance_ = msg->instance; + local_lanes.instance = rubis::instance_; + local_lanes.pose = msg->pose; + local_lanes.twist = msg->twist; + + pub_LocalTrajectoriesWithPoseTwist.publish(local_lanes); + pub_LocalTrajectories.publish(local_lanes.lane_array); + + } + else{ + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); + + visualization_msgs::MarkerArray all_rollOuts; + PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts); + pub_LocalTrajectoriesRviz.publish(all_rollOuts); + + rubis::stop_task_profiling(0, 0); + } +} + + +void TrajectoryGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) +{ + m_VehicleStatus.speed = msg->speed/3.6; + m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; +} + +void TrajectoryGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) +{ + m_VehicleStatus.speed = msg->twist.twist.linear.x; + m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; +} + +void TrajectoryGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) +{ + if(msg->lanes.size() > 0) + { + bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); + + m_GlobalPaths.clear(); + + for(unsigned int i = 0 ; i < msg->lanes.size(); i++) + { + PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); + + PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); + m_GlobalPaths.push_back(m_temp_path); + + if(bOldGlobalPath) + { + bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); + } + } + + if(!bOldGlobalPath) + { + bWayGlobalPath = true; + std::cout << "Received New Global Path Generator ! " << std::endl; + } + else + { + m_GlobalPaths.clear(); + } + } +} + +void TrajectoryGen::MainLoop() +{ + ros::NodeHandle private_nh("~"); + + // Scheduling Setup + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; + double task_execution_time; + double task_relative_deadline; + + private_nh.param("/op_trajectory_generator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv"); + private_nh.param("/op_trajectory_generator/rate", rate, 10); + private_nh.param("/op_trajectory_generator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + private_nh.param("/op_trajectory_generator/task_execution_time", task_execution_time, (double)10); + private_nh.param("/op_trajectory_generator/task_relative_deadline", task_relative_deadline, (double)10); + + rubis::init_task_profiling(task_response_time_filename); + + PlannerHNS::WayPoint prevState, state_change; + + ros::spin(); +} + +} \ No newline at end of file diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/package.xml b/autoware.ai/src/autoware/core_planning/op_local_planner/package.xml index e638fb94..9bf6a197 100644 --- a/autoware.ai/src/autoware/core_planning/op_local_planner/package.xml +++ b/autoware.ai/src/autoware/core_planning/op_local_planner/package.xml @@ -26,4 +26,5 @@ libwaypoint_follower yaml-cpp rubis_lib + rubis_msgs diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h index 6222e31d..4e2511ef 100755 --- a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h +++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h @@ -39,8 +39,6 @@ #include #include -#include - #include #include @@ -91,8 +89,6 @@ class PurePursuitNode ros::NodeHandle nh_; ros::NodeHandle private_nh_; - std::shared_ptr health_checker_ptr_; - // class PurePursuit pp_; diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch index f9212a6a..3ac38797 100644 --- a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch +++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch @@ -14,7 +14,7 @@ - + diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp index 1fa981fd..0532fe93 100755 --- a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp +++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp @@ -45,9 +45,6 @@ PurePursuitNode::PurePursuitNode() , minimum_lookahead_distance_(6.0) { initForROS(); - health_checker_ptr_ = - std::make_shared(nh_, private_nh_); - health_checker_ptr_->ENABLE(); // initialize for PurePursuit pp_.setLinearInterpolationParameter(is_linear_interpolation_); } @@ -268,9 +265,6 @@ void PurePursuitNode::run() publishTwistStamped(can_get_curvature, kappa); publishControlCommandStamped(can_get_curvature, kappa); - health_checker_ptr_->NODE_ACTIVATE(); - health_checker_ptr_->CHECK_RATE("topic_rate_vehicle_cmd_slow", 8, 5, 1, - "topic vehicle_cmd publish rate slow."); // for visualization with Rviz pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint())); pub13_.publish(displaySearchRadius( diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h b/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h index 1355c302..997b984c 100644 --- a/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h +++ b/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h @@ -17,11 +17,11 @@ #ifndef TWIST_FILTER_TWIST_FILTER_NODE_H #define TWIST_FILTER_TWIST_FILTER_NODE_H +#include #include "twist_filter/twist_filter.h" #include #include #include -#include #include #include #include @@ -38,7 +38,6 @@ class TwistFilterNode private: ros::NodeHandle nh_, private_nh_; std::shared_ptr twist_filter_ptr_; - autoware_health_checker::HealthChecker health_checker_; // publishers ros::Publisher twist_pub_, rubis_twist_pub_, ctrl_pub_; diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp index f6e797fd..006a025f 100644 --- a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp +++ b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp @@ -18,7 +18,7 @@ namespace twist_filter_node { -TwistFilterNode::TwistFilterNode() : nh_(), private_nh_("~"), health_checker_(nh_, private_nh_) +TwistFilterNode::TwistFilterNode() : nh_(), private_nh_("~") { // Parameters twist_filter::Configuration twist_filter_config; @@ -34,9 +34,6 @@ TwistFilterNode::TwistFilterNode() : nh_(), private_nh_("~"), health_checker_(nh emergency_stop_ = false; current_stop_count_ = 0; - // Enable health checker - health_checker_.ENABLE(); - // Subscribe if(rubis::instance_mode_) rubis_twist_sub_ = nh_.subscribe("rubis_twist_raw", 1, &TwistFilterNode::rubisTwistCmdCallback, this); else twist_sub_ = nh_.subscribe("twist_raw", 1, &TwistFilterNode::twistCmdCallback, this); @@ -85,9 +82,6 @@ inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConst double time_elapsed = (current_time - last_callback_time).toSec(); - health_checker_.NODE_ACTIVATE(); - checkTwist(twist, twist_prev, time_elapsed); - twist_filter::Twist twist_out = twist; // Apply lateral limit @@ -123,7 +117,7 @@ inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConst out_msg.twist.angular.z = twist_out.az; } else{ - out_msg.twist.linear.x = -1000; + out_msg.twist.linear.x = 0; out_msg.twist.angular.z = 0; } twist_pub_.publish(out_msg); @@ -181,7 +175,6 @@ void TwistFilterNode::ctrlCmdCallback(const autoware_msgs::ControlCommandStamped double time_elapsed = (current_time - last_callback_time).toSec(); - health_checker_.NODE_ACTIVATE(); checkCtrl(ctrl, ctrl_prev, time_elapsed); twist_filter::Ctrl ctrl_out = ctrl; @@ -275,18 +268,6 @@ void TwistFilterNode::checkTwist(const twist_filter::Twist twist, const twist_fi const twist_filter::Configuration& config = twist_filter_ptr_->getConfiguration(); - if (lacc) - { - health_checker_.CHECK_MAX_VALUE("twist_lateral_accel_high", lacc.get(), config.lateral_accel_limit, - 2 * config.lateral_accel_limit, DBL_MAX, - "lateral_accel is too high in twist filtering"); - } - if (ljerk) - { - health_checker_.CHECK_MAX_VALUE("twist_lateral_jerk_high", lacc.get(), config.lateral_jerk_limit, - 2 * config.lateral_jerk_limit, DBL_MAX, - "lateral_jerk is too high in twist filtering"); - } } void TwistFilterNode::checkCtrl(const twist_filter::Ctrl ctrl, const twist_filter::Ctrl ctrl_prev, const double& dt) @@ -296,18 +277,6 @@ void TwistFilterNode::checkCtrl(const twist_filter::Ctrl ctrl, const twist_filte const twist_filter::Configuration& config = twist_filter_ptr_->getConfiguration(); - if (lacc) - { - health_checker_.CHECK_MAX_VALUE("ctrl_lateral_accel_high", lacc.get(), config.lateral_accel_limit, - 3 * config.lateral_accel_limit, DBL_MAX, - "lateral_accel is too high in ctrl filtering"); - } - if (ljerk) - { - health_checker_.CHECK_MAX_VALUE("ctrl_lateral_jerk_high", lacc.get(), config.lateral_jerk_limit, - 3 * config.lateral_jerk_limit, DBL_MAX, - "lateral_jerk is too high in ctrl filtering"); - } } } // namespace twist_filter_node diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h b/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h index a5883ecb..ba9dae2c 100644 --- a/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h +++ b/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h @@ -42,10 +42,6 @@ #include "tablet_socket_msgs/gear_cmd.h" #include "tablet_socket_msgs/mode_cmd.h" -// headers in Autowae Health Checker -#include - - extern int zero_flag_; class TwistGate @@ -86,7 +82,6 @@ class TwistGate ros::NodeHandle nh_; ros::NodeHandle private_nh_; - std::shared_ptr health_checker_ptr_; ros::Publisher control_command_pub_; ros::Publisher vehicle_cmd_pub_; ros::Publisher rubis_vehicle_cmd_pub_; diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp index 14324122..d008743b 100644 --- a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp +++ b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp @@ -50,7 +50,6 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n private_nh_.param("use_decision_maker", use_decision_maker_, false); private_nh_.param("instance_mode", rubis::instance_mode_, 0); - health_checker_ptr_ = std::make_shared(nh_, private_nh_); control_command_pub_ = nh_.advertise("/ctrl_mode", 1); vehicle_cmd_pub_ = nh_.advertise("/vehicle_cmd", 1, true); if(rubis::instance_mode_) rubis_vehicle_cmd_pub_ = nh_.advertise("/rubis_vehicle_cmd", 1, true); @@ -76,8 +75,6 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n twist_gate_msg_.header.seq = 0; emergency_stop_msg_.data = false; - health_checker_ptr_->ENABLE(); - health_checker_ptr_->NODE_ACTIVATE(); remote_cmd_time_ = ros::Time::now(); emergency_handling_time_ = ros::Time::now(); @@ -154,14 +151,10 @@ void TwistGate::watchdogTimer() if (command_mode_ == CommandMode::REMOTE) { const double dt = (now_time - remote_cmd_time_).toSec() * 1000; - health_checker_ptr_->CHECK_MAX_VALUE("remote_cmd_interval", dt, 700, 1000, 1500, "remote cmd interval is too " - "long."); } // check push emergency stop button const int level = (emergency_stop_msg_.data) ? AwDiagStatus::ERROR : AwDiagStatus::OK; - health_checker_ptr_->CHECK_TRUE("emergency_stop_button", emergency_stop_msg_.data, level, "emergency stop button " - "is pushed."); // if no emergency message received for more than timeout_period_ if ((now_time - emergency_handling_time_) > timeout_period_) @@ -211,10 +204,6 @@ void TwistGate::autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::Con } inline void TwistGate::updateTwistGateMsg(const geometry_msgs::TwistStamped::ConstPtr& input_msg){ - health_checker_ptr_->CHECK_RATE("topic_rate_twist_cmd_slow", 8, 5, 1, "topic twist_cmd subscribe rate slow."); - health_checker_ptr_->CHECK_MAX_VALUE("twist_cmd_linear_high", input_msg->twist.linear.x, - DBL_MAX, DBL_MAX, DBL_MAX, "linear twist_cmd is too high"); - if (command_mode_ == CommandMode::AUTO && !emergency_handling_active_) { twist_gate_msg_.header.frame_id = input_msg->header.frame_id; diff --git a/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst b/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst new file mode 100644 index 00000000..41c70f27 --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst @@ -0,0 +1,267 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nav_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.13.1 (2021-01-11) +------------------- +* Update package maintainers (`#168 `_) +* Add LoadMap service (`#164 `_) +* Contributors: David V. Lu!!, Michel Hidalgo + +1.13.0 (2020-05-21) +------------------- +* Bump CMake version to avoid CMP0048 warning (`#158 `_) +* Contributors: Shane Loretz + +1.12.7 (2018-11-06) +------------------- + +1.12.6 (2018-05-03) +------------------- + +1.12.5 (2016-09-30) +------------------- + +1.12.4 (2016-02-22) +------------------- + +1.12.3 (2015-04-20) +------------------- + +1.12.2 (2015-03-21) +------------------- +* change type of initial_pose in SetMap service to PoseWithCovarianceStamped +* Contributors: Stephan Wirth + +1.12.1 (2015-03-17) +------------------- +* updating outdated urls. fixes `#52 `_. +* Adds a SetMap service message to support swap maps functionality in amcl +* Contributors: Tully Foote, liz-murphy + +1.12.0 (2014-12-29) +------------------- + +1.11.6 (2014-11-04) +------------------- + +1.11.5 (2014-10-27) +------------------- + +1.11.4 (2014-06-19) +------------------- + +1.11.3 (2014-05-07) +------------------- +* Export architecture_independent flag in package.xml +* Contributors: Scott K Logan + +1.11.2 (2014-04-24) +------------------- + +1.11.1 (2014-04-16) +------------------- + +1.11.0 (2014-03-04) +------------------- + +1.10.6 (2014-02-27) +------------------- + +1.10.5 (2014-02-25) +------------------- + +1.10.4 (2014-02-18) +------------------- + +1.10.3 (2014-01-07) +------------------- + +1.10.2 (2013-08-19) +------------------- + +1.10.1 (2013-08-16) +------------------- + +1.10.0 (2013-07-13) +------------------- + +1.9.16 (2013-05-21) +------------------- +* added action definition for getting maps +* update email in package.xml + +1.9.15 (2013-03-08) +------------------- + +1.9.14 (2013-01-19) +------------------- + +1.9.13 (2013-01-13) +------------------- + +1.9.12 (2013-01-02) +------------------- + +1.9.11 (2012-12-17) +------------------- +* modified dep type of catkin + +1.9.10 (2012-12-13) +------------------- +* add missing downstream depend +* switched from langs to message_* packages + +1.9.9 (2012-11-22) +------------------ + +1.9.8 (2012-11-14) +------------------ + +1.9.7 (2012-10-30) +------------------ +* fix catkin function order + +1.9.6 (2012-10-18) +------------------ +* updated cmake min version to 2.8.3, use cmake_parse_arguments instead of custom macro + +1.9.5 (2012-09-28) +------------------ + +1.9.4 (2012-09-27 18:06) +------------------------ + +1.9.3 (2012-09-27 17:39) +------------------------ +* cleanup +* cleaned up package.xml files +* updated to latest catkin +* fixed dependencies and more +* updated to latest catkin: created package.xmls, updated CmakeLists.txt + +1.9.2 (2012-09-05) +------------------ +* updated pkg-config in manifest.xml + +1.9.1 (2012-09-04) +------------------ +* use install destination variables, removed manual installation of manifests + +1.9.0 (2012-08-29) +------------------ +* updated to current catkin + +1.8.13 (2012-07-26 18:34:15 +0000) +---------------------------------- + +1.8.8 (2012-06-12 22:36) +------------------------ +* make find_package REQUIRED +* removed obsolete catkin tag from manifest files +* fixed package dependencies for several common messages (fixed `#3956 `_) +* adding manifest exports +* removed depend, added catkin +* stripping depend and export tags from common_msgs manifests as msg dependencies are now declared in cmake and stack.yaml. Also removed bag migration exports +* common_msgs: removing migration rules as all are over a year old +* bye bye vestigial MSG_DIRS +* nav_msgs: getting rid of other build files and cleaning up +* common_msgs: starting catkin conversion +* adios rosbuild2 in manifest.xml +* catkin updates +* catkin_project +* Updated to work with new message generation macros +* More tweaking for standalone message generation +* Getting standalone message generation working... w/o munging rosbuild2 +* more rosbuild2 hacking +* missing dependencies +* updating bagmigration exports +* rosbuild2 taking shape +* removing old exports ros`#2292 `_ +* Added Ubuntu platform tags to manifest +* Adding a start pose to the GetPlan service +* Remove use of deprecated rosbuild macros +* Fixing migration rules for nav_msgs. +* Changed byte to int8, in response to map_server doc review +* changing review status +* adding documentation for `#2997 `_ +* removing redundant range statements as per ticket:2997 +* Adding documentation to the Odometry message to make things more clear +* manifest update +* updated description and url +* full migration rules +* adding child_frame_id as per discussion about odometry message +* Adding a header to Path +* Adding a header to the GridCells message +* Adding a new GridCells message for displaying obstacles in nav_view and rviz +* clearing API reviews for they've been through a bunch of them recently. +* fixing stack name +* Adding comments to path +* documenting messages +* Making odometry migration fail until we have worked out appropriate way to handle covariances. +* Changing naming of bag migration rules. +* Modifying migration rules for Odometry and WrenchStamped change of field names. +* Adding actual migration rules for all of the tested common_msgs migrations. +* `#2250 `_ getting rid of _with_covariance in Odometry fields +* nav_msgs: added missing srv export +* Adding migration rules to get migration tests to pass. +* removing last of robot_msgs and all dependencies on it +* moving Path from robot_msgs to nav_msgs `#2281 `_ +* adding header to OccupancyGrid `#1906 `_ +* First half of the change from deprecated_msgs::RobotBase2DOdom to nav_msgs::Odometry, I think all the c++ compiles, can't speak for functionality yet, also... the python has yet to be run... this may break some things +* moving PoseArray into geometry_msgs `#1907 `_ +* fixing names +* Removing header since there's already one in the pose and fixing message definition to have variable names +* adding Odometry message as per API review and ticket:2250 +* merging in the changes to messages see ros-users email. THis is about half the common_msgs API changes +* Forgot to check in the services I added.... shoot +* Moving StaticMap.srv to GetMap.srv and moving Plan.srv to GetPlan.srv, also moving them to nav_msgs and removing the nav_srvs package +* Merging tha actionlib branch back into trunk + r29135@att (orig r19792): eitanme | 2009-07-27 18:30:30 -0700 + Creating a branch for actionlib.... hopefully for the last time + r29137@att (orig r19794): eitanme | 2009-07-27 18:32:49 -0700 + Changing ParticleCloud to PoseArray + r29139@att (orig r19796): eitanme | 2009-07-27 18:33:42 -0700 + Adding action definition to the rep + r29148@att (orig r19805): eitanme | 2009-07-27 18:47:39 -0700 + Some fixes... almost compiling + r29165@att (orig r19822): eitanme | 2009-07-27 20:41:07 -0700 + Macro version of the typedefs that compiles + r29213@att (orig r19869): eitanme | 2009-07-28 11:49:10 -0700 + Compling version of the ActionServer re-write complete with garbage collection, be default it will keep goals without goal handles for 5 seconds + r29220@att (orig r19876): eitanme | 2009-07-28 12:06:06 -0700 + Fix to make sure that transitions into preempting and recalling states actually happen + r29254@att (orig r19888): eitanme | 2009-07-28 13:27:40 -0700 + Forgot to actually call the cancel callback... addind a subscriber on the cancel topic + r29267@att (orig r19901): eitanme | 2009-07-28 14:41:09 -0700 + Adding text field to GoalStatus to allow users to throw some debugging information into the GoalStatus messages + r29275@att (orig r19909): eitanme | 2009-07-28 15:43:49 -0700 + Using tf remapping as I should've been doing for awhile + r29277@att (orig r19911): eitanme | 2009-07-28 15:48:48 -0700 + The navigation stack can now handle goals that aren't in the global frame. However, these goals will be transformed to the global frame at the time of reception, so for achieving them accurately the global frame of move_base should really be set to match the goals. + r29299@att (orig r19933): stuglaser | 2009-07-28 17:08:10 -0700 + Created genaction.py script to create the various messages that an action needs + r29376@att (orig r20003): vijaypradeep | 2009-07-29 02:45:24 -0700 + ActionClient is running. MoveBase ActionServer seems to be crashing + r29409@att (orig r20033): vijaypradeep | 2009-07-29 11:57:54 -0700 + Fixing bug with adding status trackers + r29410@att (orig r20034): vijaypradeep | 2009-07-29 11:58:18 -0700 + Changing from Release to Debug + r29432@att (orig r20056): vijaypradeep | 2009-07-29 14:07:30 -0700 + No longer building goal_manager_test.cpp + r29472@att (orig r20090): vijaypradeep | 2009-07-29 17:04:14 -0700 + Lots of Client-Side doxygen + r29484@att (orig r20101): vijaypradeep | 2009-07-29 18:35:01 -0700 + Adding to mainpage.dox + r29487@att (orig r20104): eitanme | 2009-07-29 18:55:06 -0700 + Removing file to help resolve merge I hope + r29489@att (orig r20106): eitanme | 2009-07-29 19:00:07 -0700 + Removing another file to try to resolve the branch + r29492@att (orig r20108): eitanme | 2009-07-29 19:14:25 -0700 + Again removing a file to get the merge working + r29493@att (orig r20109): eitanme | 2009-07-29 19:34:45 -0700 + Removing yet another file on which ssl negotiation fails + r29500@att (orig r20116): eitanme | 2009-07-29 19:54:18 -0700 + Fixing bug in genaction +* moving MapMetaData and OccGrid into nav_msgs `#1303 `_ +* created nav_msgs and moved ParticleCloud there `#1300 `_ diff --git a/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt b/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt new file mode 100644 index 00000000..b4751819 --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.0.2) +project(nav_msgs) + +find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs actionlib_msgs) + +add_message_files( + DIRECTORY msg + FILES + GridCells.msg + MapMetaData.msg + OccupancyGrid.msg + Odometry.msg + Path.msg) + +add_service_files( + DIRECTORY srv + FILES + GetMap.srv + GetPlan.srv + SetMap.srv + LoadMap.srv) + +add_action_files( + FILES + GetMap.action) + +generate_messages(DEPENDENCIES geometry_msgs std_msgs actionlib_msgs) + +catkin_package(CATKIN_DEPENDS geometry_msgs message_runtime std_msgs actionlib_msgs) diff --git a/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action b/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action new file mode 100644 index 00000000..080e54fe --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action @@ -0,0 +1,5 @@ +# Get the map as a nav_msgs/OccupancyGrid +--- +nav_msgs/OccupancyGrid map +--- +# no feedback \ No newline at end of file diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg new file mode 100644 index 00000000..2c928941 --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg @@ -0,0 +1,5 @@ +#an array of cells in a 2D grid +Header header +float32 cell_width +float32 cell_height +geometry_msgs/Point[] cells diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg new file mode 100644 index 00000000..398fbdd0 --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg @@ -0,0 +1,13 @@ +# This hold basic information about the characterists of the OccupancyGrid + +# The time at which the map was loaded +time map_load_time +# The map resolution [m/cell] +float32 resolution +# Map width [cells] +uint32 width +# Map height [cells] +uint32 height +# The origin of the map [m, m, rad]. This is the real-world pose of the +# cell (0,0) in the map. +geometry_msgs/Pose origin \ No newline at end of file diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg new file mode 100644 index 00000000..f2e79bda --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg @@ -0,0 +1,11 @@ +# This represents a 2-D grid map, in which each cell represents the probability of +# occupancy. + +Header header + +#MetaData for the map +MapMetaData info + +# The map data, in row-major order, starting with (0,0). Occupancy +# probabilities are in the range [0,100]. Unknown is -1. +int8[] data diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg new file mode 100644 index 00000000..73578ed8 --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg @@ -0,0 +1,7 @@ +# This represents an estimate of a position and velocity in free space. +# The pose in this message should be specified in the coordinate frame given by header.frame_id. +# The twist in this message should be specified in the coordinate frame given by the child_frame_id +Header header +string child_frame_id +geometry_msgs/PoseWithCovariance pose +geometry_msgs/TwistWithCovariance twist diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg new file mode 100644 index 00000000..979cb7d3 --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg @@ -0,0 +1,3 @@ +#An array of poses that represents a Path for a robot to follow +Header header +geometry_msgs/PoseStamped[] poses diff --git a/autoware.ai/src/autoware/messages/nav_msgs/package.xml b/autoware.ai/src/autoware/messages/nav_msgs/package.xml new file mode 100644 index 00000000..f379061f --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/package.xml @@ -0,0 +1,29 @@ + + nav_msgs + 1.13.1 + + nav_msgs defines the common messages used to interact with the + navigation stack. + + Michel Hidalgo + BSD + + http://wiki.ros.org/nav_msgs + Tully Foote + + catkin + + geometry_msgs + message_generation + std_msgs + actionlib_msgs + + geometry_msgs + message_runtime + std_msgs + actionlib_msgs + + + + + diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv new file mode 100644 index 00000000..6bd8e4a6 --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv @@ -0,0 +1,3 @@ +# Get the map as a nav_msgs/OccupancyGrid +--- +nav_msgs/OccupancyGrid map diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv new file mode 100644 index 00000000..f5c23edb --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv @@ -0,0 +1,13 @@ +# Get a plan from the current position to the goal Pose + +# The start pose for the plan +geometry_msgs/PoseStamped start + +# The final pose of the goal position +geometry_msgs/PoseStamped goal + +# If the goal is obstructed, how many meters the planner can +# relax the constraint in x and y before failing. +float32 tolerance +--- +nav_msgs/Path plan diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv new file mode 100644 index 00000000..3b9caaad --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv @@ -0,0 +1,15 @@ +# URL of map resource +# Can be an absolute path to a file: file:///path/to/maps/floor1.yaml +# Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml +string map_url +--- +# Result code defintions +uint8 RESULT_SUCCESS=0 +uint8 RESULT_MAP_DOES_NOT_EXIST=1 +uint8 RESULT_INVALID_MAP_DATA=2 +uint8 RESULT_INVALID_MAP_METADATA=3 +uint8 RESULT_UNDEFINED_FAILURE=255 + +# Returned map is only valid if result equals RESULT_SUCCESS +nav_msgs/OccupancyGrid map +uint8 result diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv new file mode 100644 index 00000000..55d3c24f --- /dev/null +++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv @@ -0,0 +1,6 @@ +# Set a new map together with an initial pose +nav_msgs/OccupancyGrid map +geometry_msgs/PoseWithCovarianceStamped initial_pose +--- +bool success + diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt b/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt index 5f862ea7..b2ae90cd 100644 --- a/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt +++ b/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt @@ -18,6 +18,7 @@ add_message_files( TwistStamped.msg VehicleCmd.msg InsStat.msg + DetectedObjectArray.msg ) generate_messages( diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg new file mode 100644 index 00000000..dae5bd5d --- /dev/null +++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg @@ -0,0 +1,2 @@ +uint64 instance +autoware_msgs/DetectedObjectArray object_array \ No newline at end of file diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg new file mode 100644 index 00000000..a7c1df6f --- /dev/null +++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg @@ -0,0 +1,5 @@ +uint64 instance +autoware_msgs/LaneArray lane_array +geometry_msgs/PoseStamped pose +geometry_msgs/TwistStamped twist +uint64 obj_instance \ No newline at end of file diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg new file mode 100644 index 00000000..05da6d2a --- /dev/null +++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg @@ -0,0 +1,5 @@ +uint64 instance +autoware_msgs/Lane lane +geometry_msgs/PoseStamped pose +geometry_msgs/TwistStamped twist +uint64 obj_instance \ No newline at end of file diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg new file mode 100644 index 00000000..03d0fa20 --- /dev/null +++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg @@ -0,0 +1,3 @@ +uint64 instance +geometry_msgs/PoseStamped pose +geometry_msgs/TwistStamped twist \ No newline at end of file diff --git a/autoware.ai/src/autoware/simulation/lgsvl_simulator_bridge/launch/bridge.launch b/autoware.ai/src/autoware/simulation/lgsvl_simulator_bridge/launch/bridge.launch index 1e669ba1..9ba29def 100644 --- a/autoware.ai/src/autoware/simulation/lgsvl_simulator_bridge/launch/bridge.launch +++ b/autoware.ai/src/autoware/simulation/lgsvl_simulator_bridge/launch/bridge.launch @@ -1,5 +1,5 @@ - + diff --git a/autoware.ai/src/autoware/utilities/autoware_camera_lidar_calibrator/launch/camera_lidar_calibration.launch b/autoware.ai/src/autoware/utilities/autoware_camera_lidar_calibrator/launch/camera_lidar_calibration.launch index 08a39312..f5fa0f9a 100644 --- a/autoware.ai/src/autoware/utilities/autoware_camera_lidar_calibrator/launch/camera_lidar_calibration.launch +++ b/autoware.ai/src/autoware/utilities/autoware_camera_lidar_calibrator/launch/camera_lidar_calibration.launch @@ -10,7 +10,7 @@ roslaunch autoware_camera_lidar_calibrator camera_lidar_calibration.launch intri - + @@ -22,16 +22,16 @@ roslaunch autoware_camera_lidar_calibrator camera_lidar_calibration.launch intri - + - + - + diff --git a/autoware.ai/src/autoware/utilities/autoware_launcher/plugins/leaf/calibration.xml b/autoware.ai/src/autoware/utilities/autoware_launcher/plugins/leaf/calibration.xml index 908bcd40..e4d91a94 100644 --- a/autoware.ai/src/autoware/utilities/autoware_launcher/plugins/leaf/calibration.xml +++ b/autoware.ai/src/autoware/utilities/autoware_launcher/plugins/leaf/calibration.xml @@ -10,7 +10,7 @@ - + diff --git a/autoware.ai/src/autoware/utilities/autoware_launcher/resources/rosbagplay.xml b/autoware.ai/src/autoware/utilities/autoware_launcher/resources/rosbagplay.xml index 91fd8cc6..3ecd8e76 100644 --- a/autoware.ai/src/autoware/utilities/autoware_launcher/resources/rosbagplay.xml +++ b/autoware.ai/src/autoware/utilities/autoware_launcher/resources/rosbagplay.xml @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/avt_camera.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/avt_camera.launch index 96e93788..4425817b 100644 --- a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/avt_camera.launch +++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/avt_camera.launch @@ -39,7 +39,7 @@ - + diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/calibration_publisher.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/calibration_publisher.launch index ea2ec96e..f29ddd60 100755 --- a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/calibration_publisher.launch +++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/calibration_publisher.launch @@ -11,7 +11,7 @@ + > diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/points2image.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/points2image.launch index bbd02004..d24c4a02 100644 --- a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/points2image.launch +++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/points2image.launch @@ -4,7 +4,7 @@ - + diff --git a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/vscan.launch b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/vscan.launch index 29657457..8d8ca68a 100755 --- a/autoware.ai/src/autoware/utilities/runtime_manager/scripts/vscan.launch +++ b/autoware.ai/src/autoware/utilities/runtime_manager/scripts/vscan.launch @@ -23,7 +23,7 @@ - + @@ -44,9 +44,9 @@ - + - + diff --git a/autoware.ai/src/autoware/visualization/detected_objects_visualizer/README.md b/autoware.ai/src/autoware/visualization/detected_objects_visualizer/README.md index 2acb4df0..7b147795 100644 --- a/autoware.ai/src/autoware/visualization/detected_objects_visualizer/README.md +++ b/autoware.ai/src/autoware/visualization/detected_objects_visualizer/README.md @@ -48,7 +48,7 @@ If you need to use it manually, be sure to add the `ROS_NAMESPACE` if using `ros For launch files, add the following line to your launch file: ```xml + ns="ROS_NAMESPACE"/> ``` i.e. to visualize the output of the `lidar tracker` use `ROS_NAMESPACE=/detection/lidar_tracker` in rosrun or diff --git a/diff2.patch b/diff2.patch new file mode 100644 index 00000000..9dc7e195 --- /dev/null +++ b/diff2.patch @@ -0,0 +1,15965 @@ +diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp +index 2aa58dfa..6958f845 100644 +--- a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp ++++ b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp +@@ -62,7 +62,7 @@ static void config_callback(const autoware_config_msgs::ConfigVoxelGridFilter::C + } + + inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstPtr& input) +-{ ++{ + pcl::PointCloud scan; + pcl::fromROSMsg(*input, scan); + +@@ -97,12 +97,10 @@ inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstP + filtered_msg.header = input->header; + filtered_points_pub.publish(filtered_msg); + +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_msgs::PointCloud2 rubis_filtered_msg; +- rubis_filtered_msg.msg = filtered_msg; +- rubis_filtered_msg.instance = rubis::instance_; +- rubis_filtered_points_pub.publish(rubis_filtered_msg); +- } ++ rubis_msgs::PointCloud2 rubis_filtered_msg; ++ rubis_filtered_msg.msg = filtered_msg; ++ rubis_filtered_msg.instance = rubis::instance_; ++ rubis_filtered_points_pub.publish(rubis_filtered_msg); + + points_downsampler_info_msg.header = input->header; + points_downsampler_info_msg.filter_name = "voxel_grid_filter"; +@@ -137,22 +135,24 @@ inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstP + << points_downsampler_info_msg.exe_time << "," + << std::endl; + } +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + } + + static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) + { +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + publish_filtered_cloud(input); + } + + static void rubis_scan_callback(const rubis_msgs::PointCloud2::ConstPtr& _input) + { ++ rubis::start_task_profiling(); ++ + sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared(_input->msg); + rubis::instance_ = _input->instance; + publish_filtered_cloud(input); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + int main(int argc, char** argv) +@@ -162,15 +162,6 @@ int main(int argc, char** argv) + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- + private_nh.param("input_topic_name", input_topic_name_, std::string("points_raw")); + private_nh.param("output_topic_name", output_topic_name_, std::string("filtered_points")); + private_nh.getParam("output_log", _output_log); +@@ -185,23 +176,32 @@ int main(int argc, char** argv) + private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); + private_nh.param("leaf_size", voxel_leaf_size, 0.1); + ++ // Scheduling & Profiling Setup + std::string node_name = ros::this_node::getName(); +- private_nh.param(node_name+"/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param(node_name+"/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voxel_grid_filter.csv"); ++ std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voexl_grid_filter.csv"); ++ ++ int rate; + private_nh.param(node_name+"/rate", rate, 10); +- private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10); +- private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); +- private_nh.param(node_name+"/instance_mode", rubis::instance_mode_, 0); + +- /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ + + // Publishers + filtered_points_pub = nh.advertise(output_topic_name_, 10); +- if(rubis::instance_mode_) +- rubis_filtered_points_pub = nh.advertise("/rubis_" + output_topic_name_, 10); ++ rubis_filtered_points_pub = nh.advertise("/rubis_" + output_topic_name_, 10); + points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); + + // Subscribers +@@ -209,47 +209,9 @@ int main(int argc, char** argv) + // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback); + ros::Subscriber scan_sub; + +- if(rubis::instance_mode_) scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 10, rubis_scan_callback); +- else scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback); +- +- /* RT Scheduling setup */ +- // ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 1, config_callback); // origin 10 +- // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 1, scan_callback); // origin 10 +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 1, rubis_scan_callback); // Def: 10 + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ ros::spin(); + + return 0; + } +diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp +new file mode 100644 +index 00000000..b931fd74 +--- /dev/null ++++ b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp +@@ -0,0 +1,211 @@ ++/* ++ * Copyright 2015-2019 Autoware Foundation. All rights reserved. ++ * ++ * Licensed under the Apache License, Version 2.0 (the "License"); ++ * you may not use this file except in compliance with the License. ++ * You may obtain a copy of the License at ++ * ++ * http://www.apache.org/licenses/LICENSE-2.0 ++ * ++ * Unless required by applicable law or agreed to in writing, software ++ * distributed under the License is distributed on an "AS IS" BASIS, ++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ++ * See the License for the specific language governing permissions and ++ * limitations under the License. ++ */ ++ ++#include ++#include ++ ++#include ++#include ++#include ++ ++#include "autoware_config_msgs/ConfigVoxelGridFilter.h" ++ ++#include ++ ++#include ++ ++#include "points_downsampler.h" ++ ++#include ++#include ++ ++#define SPIN_PROFILING ++ ++#define MAX_MEASUREMENT_RANGE 200.0 ++ ++ros::Publisher filtered_points_pub; ++ros::Publisher rubis_filtered_points_pub; ++ ++// Leaf size of VoxelGrid filter. ++static double voxel_leaf_size = 2.0; ++ ++static ros::Publisher points_downsampler_info_pub; ++static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; ++ ++static std::chrono::time_point filter_start, filter_end; ++ ++static bool _output_log = false; ++static std::ofstream ofs; ++static std::string filename; ++ ++static std::string input_topic_name_; ++static std::string output_topic_name_; ++static double measurement_range = MAX_MEASUREMENT_RANGE; ++ ++static void config_callback(const autoware_config_msgs::ConfigVoxelGridFilter::ConstPtr& input) ++{ ++ voxel_leaf_size = input->voxel_leaf_size; ++ measurement_range = input->measurement_range; ++} ++ ++inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstPtr& input) ++{ ++ pcl::PointCloud scan; ++ pcl::fromROSMsg(*input, scan); ++ ++ if(measurement_range != MAX_MEASUREMENT_RANGE){ ++ scan = removePointsByRange(scan, 0, measurement_range); ++ } ++ ++ pcl::PointCloud::Ptr scan_ptr(new pcl::PointCloud(scan)); ++ pcl::PointCloud::Ptr filtered_scan_ptr(new pcl::PointCloud()); ++ ++ sensor_msgs::PointCloud2 filtered_msg; ++ ++ filter_start = std::chrono::system_clock::now(); ++ ++ // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL) ++ if (voxel_leaf_size >= 0.1) ++ { ++ // Downsampling the velodyne scan using VoxelGrid filter ++ pcl::VoxelGrid voxel_grid_filter; ++ voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); ++ voxel_grid_filter.setInputCloud(scan_ptr); ++ voxel_grid_filter.filter(*filtered_scan_ptr); ++ pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); ++ } ++ else ++ { ++ pcl::toROSMsg(*scan_ptr, filtered_msg); ++ } ++ ++ filter_end = std::chrono::system_clock::now(); ++ ++ filtered_msg.header = input->header; ++ filtered_points_pub.publish(filtered_msg); ++ ++ rubis_msgs::PointCloud2 rubis_filtered_msg; ++ rubis_filtered_msg.msg = filtered_msg; ++ rubis_filtered_msg.instance = rubis::instance_; ++ rubis_filtered_points_pub.publish(rubis_filtered_msg); ++ ++ points_downsampler_info_msg.header = input->header; ++ points_downsampler_info_msg.filter_name = "voxel_grid_filter"; ++ points_downsampler_info_msg.measurement_range = measurement_range; ++ points_downsampler_info_msg.original_points_size = scan.size(); ++ if (voxel_leaf_size >= 0.1) ++ { ++ points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); ++ } ++ else ++ { ++ points_downsampler_info_msg.filtered_points_size = scan_ptr->size(); ++ } ++ points_downsampler_info_msg.original_ring_size = 0; ++ points_downsampler_info_msg.filtered_ring_size = 0; ++ points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; ++ points_downsampler_info_pub.publish(points_downsampler_info_msg); ++ ++ if(_output_log == true){ ++ if(!ofs){ ++ std::cerr << "Could not open " << filename << "." << std::endl; ++ exit(1); ++ } ++ ofs << points_downsampler_info_msg.header.seq << "," ++ << points_downsampler_info_msg.header.stamp << "," ++ << points_downsampler_info_msg.header.frame_id << "," ++ << points_downsampler_info_msg.filter_name << "," ++ << points_downsampler_info_msg.original_points_size << "," ++ << points_downsampler_info_msg.filtered_points_size << "," ++ << points_downsampler_info_msg.original_ring_size << "," ++ << points_downsampler_info_msg.filtered_ring_size << "," ++ << points_downsampler_info_msg.exe_time << "," ++ << std::endl; ++ } ++ ++} ++ ++static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) ++{ ++ rubis::instance_ = 0; ++ publish_filtered_cloud(input); ++} ++ ++static void rubis_scan_callback(const rubis_msgs::PointCloud2::ConstPtr& _input) ++{ ++ rubis::start_task_profiling(); ++ ++ sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared(_input->msg); ++ rubis::instance_ = _input->instance; ++ publish_filtered_cloud(input); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++} ++ ++int main(int argc, char** argv) ++{ ++ ros::init(argc, argv, "rubis_voxel_grid_filter"); ++ ++ ros::NodeHandle nh; ++ ros::NodeHandle private_nh("~"); ++ ++ // Scheduling Setup ++ std::string task_response_time_filename; ++ int rate; ++ double task_minimum_inter_release_time; ++ double task_execution_time; ++ double task_relative_deadline; ++ ++ private_nh.param("input_topic_name", input_topic_name_, std::string("points_raw")); ++ private_nh.param("output_topic_name", output_topic_name_, std::string("filtered_points")); ++ private_nh.getParam("output_log", _output_log); ++ if(_output_log == true){ ++ char buffer[80]; ++ std::time_t now = std::time(NULL); ++ std::tm *pnow = std::localtime(&now); ++ std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); ++ filename = "voxel_grid_filter_" + std::string(buffer) + ".csv"; ++ ofs.open(filename.c_str(), std::ios::app); ++ } ++ private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); ++ private_nh.param("leaf_size", voxel_leaf_size, 0.1); ++ ++ std::string node_name = ros::this_node::getName(); ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voxel_grid_filter.csv"); ++ private_nh.param(node_name+"/rate", rate, 10); ++ private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); ++ private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10); ++ private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); ++ ++ /* For Task scheduling */ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ // Publishers ++ filtered_points_pub = nh.advertise(output_topic_name_, 10); ++ rubis_filtered_points_pub = nh.advertise("/rubis_" + output_topic_name_, 10); ++ points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); ++ ++ // Subscribers ++ ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback); ++ // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback); ++ ros::Subscriber scan_sub; ++ ++ scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 10, rubis_scan_callback); ++ ++ ros::spin(); ++ ++ return 0; ++} +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h b/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h +index 18298682..5551e95e 100644 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h +@@ -78,8 +78,6 @@ private: + std::vector colors_; + const size_t color_num_ = 60; // different number of color to generate + +- int instance_mode_ = 0; +- + struct PointXYZIRTColor + { + pcl::PointXYZI point; +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch +index d4fcb70e..5418c56f 100755 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch +@@ -14,7 +14,6 @@ + + + +- + + + +@@ -31,6 +30,5 @@ + + + +- + + +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch +index 25fbed0c..eb60b4a3 100755 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch +@@ -14,7 +14,6 @@ + + + +- + + + +@@ -31,6 +30,5 @@ + + + +- + + +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp +index 92454425..d205d30f 100644 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp +@@ -221,8 +221,7 @@ void CompareMapFilter::sensorPointsCallback(const sensor_msgs::PointCloud2::Cons + return; + } + unmatch_points_pub_.publish(sensorTF_unmatch_cloud_msg); +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + } + + void CompareMapFilter::searchMatchingCloud(const pcl::PointCloud::Ptr in_cloud_ptr, +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp +index 005e14fb..e5b8bdca 100644 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp +@@ -379,8 +379,6 @@ inline void RayGroundFilter::PublishFilteredClouds(const sensor_msgs::PointCloud + publish_cloud(ground_points_pub_, ground_cloud_ptr, in_sensor_cloud->header); + publish_cloud(groundless_points_pub_, no_ground_cloud_ptr, in_sensor_cloud->header); + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; + } + + void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud) +@@ -391,9 +389,11 @@ void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_s + + void RayGroundFilter::RubisCloudCallback(const rubis_msgs::PointCloud2ConstPtr in_rubis_cloud) + { ++ rubis::start_task_profiling(); + sensor_msgs::PointCloud2ConstPtr in_sensor_cloud = boost::make_shared(in_rubis_cloud->msg); + rubis::instance_ = in_rubis_cloud->instance; + PublishFilteredClouds(in_sensor_cloud); ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + RayGroundFilter::RayGroundFilter() : node_handle_("~"), tf_listener_(tf_buffer_) +@@ -405,15 +405,6 @@ RayGroundFilter::RayGroundFilter() : node_handle_("~"), tf_listener_(tf_buffer_) + + void RayGroundFilter::Run() + { +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- + // Model | Horizontal | Vertical | FOV(Vertical) degrees / rads + // ---------------------------------------------------------- + // HDL-64 |0.08-0.35(0.32) | 0.4 | -24.9 <=x<=2.0 (26.9 / 0.47) +@@ -469,51 +460,31 @@ void RayGroundFilter::Run() + groundless_points_pub_ = node_handle_.advertise(no_ground_topic, 2); + ground_points_pub_ = node_handle_.advertise(ground_topic, 2); + ++ // Scheduling & Profiling Setup + std::string node_name = ros::this_node::getName(); +- node_handle_.param(node_name+"/task_scheduling_flag", task_scheduling_flag, 0); +- node_handle_.param(node_name+"/task_profiling_flag", task_profiling_flag, 0); ++ std::string task_response_time_filename; + node_handle_.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ray_ground_filter.csv"); +- node_handle_.param(node_name+"/rate", rate, 10); +- node_handle_.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- node_handle_.param(node_name+"/task_execution_time", task_execution_time, (double)10); +- node_handle_.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); +- node_handle_.param(node_name+"/instance_mode", instance_mode_, 0); + +- points_node_sub_ = node_handle_.subscribe("/rubis_"+input_point_topic_.substr(1), 1, &RayGroundFilter::RubisCloudCallback, this); +- +- ROS_INFO("Ready"); ++ int rate; ++ node_handle_.param(node_name+"/rate", rate, 10); + +- /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ node_handle_.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ node_handle_.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ node_handle_.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ node_handle_.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ node_handle_.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_) break; +- ros::spinOnce(); +- r.sleep(); +- } ++ rubis::init_task_profiling(task_response_time_filename); + +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ points_node_sub_ = node_handle_.subscribe("/rubis_"+input_point_topic_.substr(1), 1, &RayGroundFilter::RubisCloudCallback, this); + +- ros::spinOnce(); ++ ROS_INFO("Ready"); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ ros::spin(); + } +diff --git a/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp b/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp +index 1ced3fe7..65943460 100644 +--- a/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp ++++ b/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp +@@ -717,15 +717,13 @@ ROSRangeVisionFusionApp::Run() + ros::NodeHandle private_node_handle("~"); + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; + double task_execution_time; + double task_relative_deadline; + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + tf::TransformListener transform_listener; + +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu +index c7f2ef0b..83a0a55f 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu +@@ -156,10 +156,8 @@ __global__ void binary_gradient_array_kernel(float *x, float *dy, int n, int s, + + extern "C" void binary_gradient_array_gpu(float *x, float *dx, int n, int size, BINARY_ACTIVATION a, float *y) + { +- request_gpu(); +- binary_gradient_array_kernel<<>>(x, dx, n/2, size, a, y); +- yield_gpu_with_remark("binary_gradient_array_kernel"); +- check_error(cudaPeekAtLastError()); ++ binary_gradient_array_kernel<<>>(x, dx, n/2, size, a, y); ++ check_error(cudaPeekAtLastError()); + } + __global__ void binary_activate_array_kernel(float *x, int n, int s, BINARY_ACTIVATION a, float *y) + { +@@ -173,9 +171,7 @@ __global__ void binary_activate_array_kernel(float *x, int n, int s, BINARY_ACTI + + extern "C" void binary_activate_array_gpu(float *x, int n, int size, BINARY_ACTIVATION a, float *y) + { +- request_gpu(); +- binary_activate_array_kernel<<>>(x, n/2, size, a, y); +- yield_gpu_with_remark("binary_activate_array_kernel"); ++ binary_activate_array_kernel<<>>(x, n/2, size, a, y); + check_error(cudaPeekAtLastError()); + } + +@@ -193,16 +189,12 @@ __global__ void gradient_array_kernel(float *x, int n, ACTIVATION a, float *delt + + extern "C" void activate_array_gpu(float *x, int n, ACTIVATION a) + { +- request_gpu(); +- activate_array_kernel<<>>(x, n, a); +- yield_gpu_with_remark("activate_array_kernel"); ++ activate_array_kernel<<>>(x, n, a); + check_error(cudaPeekAtLastError()); + } + + extern "C" void gradient_array_gpu(float *x, int n, ACTIVATION a, float *delta) + { +- request_gpu(); +- gradient_array_kernel<<>>(x, n, a, delta); +- yield_gpu_with_remark("gradient_array_kernel"); ++ gradient_array_kernel<<>>(x, n, a, delta); + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu +index 5a0473bc..65a521a2 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu +@@ -47,10 +47,7 @@ extern "C" void forward_avgpool_layer_gpu(avgpool_layer layer, network net) + { + size_t n = layer.c*layer.batch; + +- request_gpu(); +- forward_avgpool_layer_kernel<<>>(n, layer.w, layer.h, layer.c, net.input_gpu, layer.output_gpu); +- yield_gpu_with_remark("forward_avgpool_layer_kernel"); +- ++ forward_avgpool_layer_kernel<<>>(n, layer.w, layer.h, layer.c, net.input_gpu, layer.output_gpu); + check_error(cudaPeekAtLastError()); + } + +@@ -58,9 +55,7 @@ extern "C" void backward_avgpool_layer_gpu(avgpool_layer layer, network net) + { + size_t n = layer.c*layer.batch; + +- request_gpu(); +- backward_avgpool_layer_kernel<<>>(n, layer.w, layer.h, layer.c, net.delta_gpu, layer.delta_gpu); +- yield_gpu_with_remark("forward_avgpool_layer_kernel"); ++ backward_avgpool_layer_kernel<<>>(n, layer.w, layer.h, layer.c, net.delta_gpu, layer.delta_gpu); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu +index 95c61fd3..0ad326d9 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu +@@ -23,9 +23,7 @@ void scale_bias_gpu(float *output, float *biases, int batch, int n, int size) + dim3 dimGrid((size-1)/BLOCK + 1, n, batch); + dim3 dimBlock(BLOCK, 1, 1); + +- request_gpu(); +- scale_bias_kernel<<>>(output, biases, n, size); +- yield_gpu_with_remark("scale_bias_kernel"); ++ scale_bias_kernel<<>>(output, biases, n, size); + + check_error(cudaPeekAtLastError()); + } +@@ -52,9 +50,7 @@ __global__ void backward_scale_kernel(float *x_norm, float *delta, int batch, in + + void backward_scale_gpu(float *x_norm, float *delta, int batch, int n, int size, float *scale_updates) + { +- request_gpu(); +- backward_scale_kernel<<>>(x_norm, delta, batch, n, size, scale_updates); +- yield_gpu_with_remark("backward_scale_kernel"); ++ backward_scale_kernel<<>>(x_norm, delta, batch, n, size, scale_updates); + + check_error(cudaPeekAtLastError()); + } +@@ -76,9 +72,7 @@ void add_bias_gpu(float *output, float *biases, int batch, int n, int size) + { + int num = n*size*batch; + +- request_gpu(); +- add_bias_kernel<<>>(output, biases, batch, n, size); +- yield_gpu_with_remark("add_bias_kernel"); ++ add_bias_kernel<<>>(output, biases, batch, n, size); + + check_error(cudaPeekAtLastError()); + } +@@ -119,13 +113,9 @@ __global__ void backward_bias_kernel(float *bias_updates, float *delta, int batc + void backward_bias_gpu(float *bias_updates, float *delta, int batch, int n, int size) + { + if(size == 1){ +- request_gpu(); +- backward_bias_conn_kernel<<>>(bias_updates, delta, batch, n); +- yield_gpu_with_remark("backward_bias_conn_kernel"); ++ backward_bias_conn_kernel<<>>(bias_updates, delta, batch, n); + }else{ +- request_gpu(); +- backward_bias_kernel<<>>(bias_updates, delta, batch, n, size); +- yield_gpu_with_remark("backward_bias_kernel"); ++ backward_bias_kernel<<>>(bias_updates, delta, batch, n, size); + } + check_error(cudaPeekAtLastError()); + } +@@ -186,9 +176,7 @@ __global__ void adam_kernel(int N, float *x, float *m, float *v, float B1, float + + extern "C" void adam_gpu(int n, float *x, float *m, float *v, float B1, float B2, float rate, float eps, int t) + { +- request_gpu(); +- adam_kernel<<>>(n, x, m, v, B1, B2, rate, eps, t); +- yield_gpu_with_remark("adam_kernel"); ++ adam_kernel<<>>(n, x, m, v, B1, B2, rate, eps, t); + + check_error(cudaPeekAtLastError()); + } +@@ -229,9 +217,7 @@ extern "C" void normalize_delta_gpu(float *x, float *mean, float *variance, floa + { + size_t N = batch*filters*spatial; + +- request_gpu(); +- normalize_delta_kernel<<>>(N, x, mean, variance, mean_delta, variance_delta, batch, filters, spatial, delta); +- yield_gpu_with_remark("normalize_delta_kernel"); ++ normalize_delta_kernel<<>>(N, x, mean, variance, mean_delta, variance_delta, batch, filters, spatial, delta); + + check_error(cudaPeekAtLastError()); + } +@@ -339,27 +325,21 @@ __global__ void mean_delta_kernel(float *delta, float *variance, int batch, int + + extern "C" void mean_delta_gpu(float *delta, float *variance, int batch, int filters, int spatial, float *mean_delta) + { +- request_gpu(); +- mean_delta_kernel<<>>(delta, variance, batch, filters, spatial, mean_delta); +- yield_gpu_with_remark("mean_delta_kernel"); ++ mean_delta_kernel<<>>(delta, variance, batch, filters, spatial, mean_delta); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void fast_mean_delta_gpu(float *delta, float *variance, int batch, int filters, int spatial, float *mean_delta) + { +- request_gpu(); +- fast_mean_delta_kernel<<>>(delta, variance, batch, filters, spatial, mean_delta); +- yield_gpu_with_remark("fast_mean_delta_kernel"); ++ fast_mean_delta_kernel<<>>(delta, variance, batch, filters, spatial, mean_delta); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void fast_variance_delta_gpu(float *x, float *delta, float *mean, float *variance, int batch, int filters, int spatial, float *variance_delta) + { +- request_gpu(); +- fast_variance_delta_kernel<<>>(x, delta, mean, variance, batch, filters, spatial, variance_delta); +- yield_gpu_with_remark("fast_variance_delta_kernel"); ++ fast_variance_delta_kernel<<>>(x, delta, mean, variance, batch, filters, spatial, variance_delta); + + check_error(cudaPeekAtLastError()); + } +@@ -495,9 +475,7 @@ extern "C" void normalize_gpu(float *x, float *mean, float *variance, int batch, + { + size_t N = batch*filters*spatial; + +- request_gpu(); +- normalize_kernel<<>>(N, x, mean, variance, batch, filters, spatial); +- yield_gpu_with_remark("normalize_kernel"); ++ normalize_kernel<<>>(N, x, mean, variance, batch, filters, spatial); + + check_error(cudaPeekAtLastError()); + } +@@ -528,9 +506,7 @@ extern "C" void l2normalize_gpu(float *x, float *dx, int batch, int filters, int + { + size_t N = batch*spatial; + +- request_gpu(); +- l2norm_kernel<<>>(N, x, dx, batch, filters, spatial); +- yield_gpu_with_remark("l2norm_kernel"); ++ l2norm_kernel<<>>(N, x, dx, batch, filters, spatial); + + check_error(cudaPeekAtLastError()); + } +@@ -596,18 +572,14 @@ __global__ void fast_variance_kernel(float *x, float *mean, int batch, int filt + + extern "C" void fast_mean_gpu(float *x, int batch, int filters, int spatial, float *mean) + { +- request_gpu(); +- fast_mean_kernel<<>>(x, batch, filters, spatial, mean); +- yield_gpu_with_remark("fast_mean_kernel"); ++ fast_mean_kernel<<>>(x, batch, filters, spatial, mean); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void fast_variance_gpu(float *x, float *mean, int batch, int filters, int spatial, float *variance) + { +- request_gpu(); +- fast_variance_kernel<<>>(x, mean, batch, filters, spatial, variance); +- yield_gpu_with_remark("fast_mean_kernel"); ++ fast_variance_kernel<<>>(x, mean, batch, filters, spatial, variance); + + check_error(cudaPeekAtLastError()); + } +@@ -615,18 +587,14 @@ extern "C" void fast_variance_gpu(float *x, float *mean, int batch, int filters, + + extern "C" void mean_gpu(float *x, int batch, int filters, int spatial, float *mean) + { +- request_gpu(); +- mean_kernel<<>>(x, batch, filters, spatial, mean); +- yield_gpu_with_remark("mean_kernel"); ++ mean_kernel<<>>(x, batch, filters, spatial, mean); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void variance_gpu(float *x, float *mean, int batch, int filters, int spatial, float *variance) + { +- request_gpu(); +- variance_kernel<<>>(x, mean, batch, filters, spatial, variance); +- yield_gpu_with_remark("variance_kernel"); ++ variance_kernel<<>>(x, mean, batch, filters, spatial, variance); + + check_error(cudaPeekAtLastError()); + } +@@ -638,18 +606,14 @@ extern "C" void axpy_gpu(int N, float ALPHA, float * X, int INCX, float * Y, int + + extern "C" void pow_gpu(int N, float ALPHA, float * X, int INCX, float * Y, int INCY) + { +- request_gpu(); +- pow_kernel<<>>(N, ALPHA, X, INCX, Y, INCY); +- yield_gpu_with_remark("pow_kernel"); ++ pow_kernel<<>>(N, ALPHA, X, INCX, Y, INCY); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void axpy_gpu_offset(int N, float ALPHA, float * X, int OFFX, int INCX, float * Y, int OFFY, int INCY) + { +- request_gpu(); +- axpy_kernel<<>>(N, ALPHA, X, OFFX, INCX, Y, OFFY, INCY); +- yield_gpu_with_remark("pow_kernel"); ++ axpy_kernel<<>>(N, ALPHA, X, OFFX, INCX, Y, OFFY, INCY); + + check_error(cudaPeekAtLastError()); + } +@@ -661,18 +625,14 @@ extern "C" void copy_gpu(int N, float * X, int INCX, float * Y, int INCY) + + extern "C" void mul_gpu(int N, float * X, int INCX, float * Y, int INCY) + { +- request_gpu(); +- mul_kernel<<>>(N, X, INCX, Y, INCY); +- yield_gpu_with_remark("mul_kernel"); ++ mul_kernel<<>>(N, X, INCX, Y, INCY); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void copy_gpu_offset(int N, float * X, int OFFX, int INCX, float * Y, int OFFY, int INCY) + { +- request_gpu(); +- copy_kernel<<>>(N, X, OFFX, INCX, Y, OFFY, INCY); +- yield_gpu_with_remark("copy_kernel"); ++ copy_kernel<<>>(N, X, OFFX, INCX, Y, OFFY, INCY); + + check_error(cudaPeekAtLastError()); + } +@@ -698,9 +658,7 @@ extern "C" void flatten_gpu(float *x, int spatial, int layers, int batch, int fo + { + int size = spatial*batch*layers; + +- request_gpu(); +- flatten_kernel<<>>(size, x, spatial, layers, batch, forward, out); +- yield_gpu_with_remark("flatten_kernel"); ++ flatten_kernel<<>>(size, x, spatial, layers, batch, forward, out); + + check_error(cudaPeekAtLastError()); + } +@@ -709,9 +667,7 @@ extern "C" void reorg_gpu(float *x, int w, int h, int c, int batch, int stride, + { + int size = w*h*c*batch; + +- request_gpu(); +- reorg_kernel<<>>(size, x, w, h, c, batch, stride, forward, out); +- yield_gpu_with_remark("reorg_kernel"); ++ reorg_kernel<<>>(size, x, w, h, c, batch, stride, forward, out); + + check_error(cudaPeekAtLastError()); + } +@@ -724,9 +680,7 @@ __global__ void mask_kernel(int n, float *x, float mask_num, float *mask, float + + extern "C" void mask_gpu(int N, float * X, float mask_num, float * mask, float val) + { +- request_gpu(); +- mask_kernel<<>>(N, X, mask_num, mask, val); +- yield_gpu_with_remark("mask_kernel"); ++ mask_kernel<<>>(N, X, mask_num, mask, val); + + check_error(cudaPeekAtLastError()); + } +@@ -739,27 +693,21 @@ __global__ void scale_mask_kernel(int n, float *x, float mask_num, float *mask, + + extern "C" void scale_mask_gpu(int N, float * X, float mask_num, float * mask, float scale) + { +- request_gpu(); +- scale_mask_kernel<<>>(N, X, mask_num, mask, scale); +- yield_gpu_with_remark("scale_mask_kernel"); ++ scale_mask_kernel<<>>(N, X, mask_num, mask, scale); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void const_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- const_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("const_kernel"); ++ const_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void constrain_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- constrain_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("constrain_kernel"); ++ constrain_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } +@@ -767,36 +715,28 @@ extern "C" void constrain_gpu(int N, float ALPHA, float * X, int INCX) + + extern "C" void add_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- add_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("add_kernel"); ++ add_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void scal_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- scal_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("scal_kernel"); ++ scal_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void supp_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- supp_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("supp_kernel"); ++ supp_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void fill_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- fill_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("fill_kernel"); ++ fill_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } +@@ -834,9 +774,7 @@ extern "C" void shortcut_gpu(int batch, int w1, int h1, int c1, float *add, int + + int size = batch * minw * minh * minc; + +- request_gpu(); +- shortcut_kernel<<>>(size, minw, minh, minc, stride, sample, batch, w1, h1, c1, add, w2, h2, c2, s1, s2, out); +- yield_gpu_with_remark("shortcut_kernel"); ++ shortcut_kernel<<>>(size, minw, minh, minc, stride, sample, batch, w1, h1, c1, add, w2, h2, c2, s1, s2, out); + + check_error(cudaPeekAtLastError()); + } +@@ -860,9 +798,7 @@ __global__ void smooth_l1_kernel(int n, float *pred, float *truth, float *delta, + + extern "C" void smooth_l1_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- smooth_l1_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("smooth_l1_kernel"); ++ smooth_l1_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -880,9 +816,7 @@ __global__ void softmax_x_ent_kernel(int n, float *pred, float *truth, float *de + + extern "C" void softmax_x_ent_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- softmax_x_ent_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("softmax_x_ent_kernel"); ++ softmax_x_ent_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -900,9 +834,7 @@ __global__ void logistic_x_ent_kernel(int n, float *pred, float *truth, float *d + + extern "C" void logistic_x_ent_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- logistic_x_ent_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("logistic_x_ent_kernel"); ++ logistic_x_ent_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -919,9 +851,7 @@ __global__ void l2_kernel(int n, float *pred, float *truth, float *delta, float + + extern "C" void l2_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- l2_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("l2_kernel"); ++ l2_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -938,9 +868,7 @@ __global__ void l1_kernel(int n, float *pred, float *truth, float *delta, float + + extern "C" void l1_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- l1_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("l1_kernel"); ++ l1_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -956,9 +884,7 @@ __global__ void wgan_kernel(int n, float *pred, float *truth, float *delta, floa + + extern "C" void wgan_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- wgan_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("wgan_kernel"); ++ wgan_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -990,9 +916,7 @@ __global__ void deinter_kernel(int NX, float *X, int NY, float *Y, int B, float + + extern "C" void deinter_gpu(int NX, float *X, int NY, float *Y, int B, float *OUT) + { +- request_gpu(); +- deinter_kernel<<>>(NX, X, NY, Y, B, OUT); +- yield_gpu_with_remark("deinter_kernel"); ++ deinter_kernel<<>>(NX, X, NY, Y, B, OUT); + + check_error(cudaPeekAtLastError()); + } +@@ -1013,18 +937,14 @@ __global__ void inter_kernel(int NX, float *X, int NY, float *Y, int B, float *O + + extern "C" void inter_gpu(int NX, float *X, int NY, float *Y, int B, float *OUT) + { +- request_gpu(); +- inter_kernel<<>>(NX, X, NY, Y, B, OUT); +- yield_gpu_with_remark("inter_kernel"); ++ inter_kernel<<>>(NX, X, NY, Y, B, OUT); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void weighted_sum_gpu(float *a, float *b, float *s, int num, float *c) + { +- request_gpu(); +- weighted_sum_kernel<<>>(num, a, b, s, c); +- yield_gpu_with_remark("weighted_sum_kernel"); ++ weighted_sum_kernel<<>>(num, a, b, s, c); + + check_error(cudaPeekAtLastError()); + } +@@ -1041,9 +961,7 @@ __global__ void weighted_delta_kernel(int n, float *a, float *b, float *s, float + + extern "C" void weighted_delta_gpu(float *a, float *b, float *s, float *da, float *db, float *ds, int num, float *dc) + { +- request_gpu(); +- weighted_delta_kernel<<>>(num, a, b, s, da, db, ds, dc); +- yield_gpu_with_remark("weighted_delta_kernel"); ++ weighted_delta_kernel<<>>(num, a, b, s, da, db, ds, dc); + + check_error(cudaPeekAtLastError()); + } +@@ -1058,9 +976,7 @@ __global__ void mult_add_into_kernel(int n, float *a, float *b, float *c) + + extern "C" void mult_add_into_gpu(int num, float *a, float *b, float *c) + { +- request_gpu(); +- mult_add_into_kernel<<>>(num, a, b, c); +- yield_gpu_with_remark("mult_add_into_kernel"); ++ mult_add_into_kernel<<>>(num, a, b, c); + + check_error(cudaPeekAtLastError()); + } +@@ -1113,9 +1029,7 @@ extern "C" void softmax_tree(float *input, int spatial, int batch, int stride, f + */ + int num = spatial*batch*hier.groups; + +- request_gpu(); +- softmax_tree_kernel<<>>(input, spatial, batch, stride, temp, output, hier.groups, tree_groups_size, tree_groups_offset); +- yield_gpu_with_remark("softmax_tree_kernel"); ++ softmax_tree_kernel<<>>(input, spatial, batch, stride, temp, output, hier.groups, tree_groups_size, tree_groups_offset); + + check_error(cudaPeekAtLastError()); + cuda_free((float *)tree_groups_size); +@@ -1133,9 +1047,7 @@ __global__ void softmax_kernel(float *input, int n, int batch, int batch_offset, + + extern "C" void softmax_gpu(float *input, int n, int batch, int batch_offset, int groups, int group_offset, int stride, float temp, float *output) + { +- request_gpu(); +- softmax_kernel<<>>(input, n, batch, batch_offset, groups, group_offset, stride, temp, output); +- yield_gpu_with_remark("softmax_tree_kernel"); ++ softmax_kernel<<>>(input, n, batch, batch_offset, groups, group_offset, stride, temp, output); + + check_error(cudaPeekAtLastError()); + } +@@ -1168,9 +1080,7 @@ extern "C" void upsample_gpu(float *in, int w, int h, int c, int batch, int stri + { + size_t size = w*h*c*batch*stride*stride; + +- request_gpu(); +- upsample_kernel<<>>(size, in, w, h, c, batch, stride, forward, scale, out); +- yield_gpu_with_remark("upsample_kernel"); ++ upsample_kernel<<>>(size, in, w, h, c, batch, stride, forward, scale, out); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu +index ad00edde..64a35528 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu +@@ -50,12 +50,10 @@ void col2im_gpu(float *data_col, + int width_col = (width + 2 * pad - ksize) / stride + 1; + int num_kernels = channels * height * width; + +- request_gpu(); +- col2im_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK, ++ col2im_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK, + BLOCK>>>( + num_kernels, data_col, height, width, ksize, pad, + stride, height_col, + width_col, data_im); +- yield_gpu_with_remark("col2im_gpu_kernel"); + } + +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu +index 7792dc81..95f07477 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu +@@ -22,9 +22,7 @@ __global__ void binarize_kernel(float *x, int n, float *binary) + + void binarize_gpu(float *x, int n, float *binary) + { +- request_gpu(); +- binarize_kernel<<>>(x, n, binary); +- yield_gpu_with_remark("binarize_kernel"); ++ binarize_kernel<<>>(x, n, binary); + + check_error(cudaPeekAtLastError()); + } +@@ -46,9 +44,7 @@ __global__ void binarize_input_kernel(float *input, int n, int size, float *bina + + void binarize_input_gpu(float *input, int n, int size, float *binary) + { +- request_gpu(); +- binarize_input_kernel<<>>(input, n, size, binary); +- yield_gpu_with_remark("binarize_input_kernel"); ++ binarize_input_kernel<<>>(input, n, size, binary); + + check_error(cudaPeekAtLastError()); + } +@@ -72,9 +68,7 @@ __global__ void binarize_weights_kernel(float *weights, int n, int size, float * + + void binarize_weights_gpu(float *weights, int n, int size, float *binary) + { +- request_gpu(); +- binarize_weights_kernel<<>>(weights, n, size, binary); +- yield_gpu_with_remark("binarize_weights_kernel"); ++ binarize_weights_kernel<<>>(weights, n, size, binary); + + check_error(cudaPeekAtLastError()); + } +@@ -177,9 +171,7 @@ extern "C" void smooth_layer(layer l, int size, float rate) + + size_t n = h*w*c*l.batch; + +- request_gpu(); +- smooth_kernel<<>>(l.output_gpu, n, l.w, l.h, l.c, size, rate, l.delta_gpu); +- yield_gpu_with_remark("smooth_kernel"); ++ smooth_kernel<<>>(l.output_gpu, n, l.w, l.h, l.c, size, rate, l.delta_gpu); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu +index d75e257d..539cfdba 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu +@@ -195,17 +195,13 @@ extern "C" void forward_crop_layer_gpu(crop_layer layer, network net) + + int size = layer.batch * layer.w * layer.h; + +- request_gpu(); +- levels_image_kernel<<>>(net.input_gpu, layer.rand_gpu, layer.batch, layer.w, layer.h, net.train, layer.saturation, layer.exposure, translate, scale, layer.shift); +- yield_gpu_with_remark("levels_image_kernel"); ++ levels_image_kernel<<>>(net.input_gpu, layer.rand_gpu, layer.batch, layer.w, layer.h, net.train, layer.saturation, layer.exposure, translate, scale, layer.shift); + + check_error(cudaPeekAtLastError()); + + size = layer.batch*layer.c*layer.out_w*layer.out_h; + +- request_gpu(); +- forward_crop_layer_kernel<<>>(net.input_gpu, layer.rand_gpu, size, layer.c, layer.h, layer.w, layer.out_h, layer.out_w, net.train, layer.flip, radians, layer.output_gpu); +- yield_gpu_with_remark("forward_crop_layer_kernel"); ++ forward_crop_layer_kernel<<>>(net.input_gpu, layer.rand_gpu, size, layer.c, layer.h, layer.w, layer.out_h, layer.out_w, net.train, layer.flip, radians, layer.output_gpu); + + check_error(cudaPeekAtLastError()); + +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c +index a9da26f1..5c1bfc0d 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c +@@ -92,15 +92,11 @@ float *cuda_make_array(float *x, size_t n) + float *x_gpu; + size_t size = sizeof(float)*n; + +- request_gpu(); +- cudaError_t status = cudaMalloc((void **)&x_gpu, size); +- yield_gpu_with_remark("cudaMalloc"); ++ cudaError_t status = cudaMalloc((void **)&x_gpu, size); + + check_error(status); + if(x){ +- request_gpu(); +- status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); +- yield_gpu_with_remark("cuda_make_array"); ++ status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); + check_error(status); + } else { + fill_gpu(n, 0, x_gpu, 1); +@@ -114,25 +110,17 @@ void cuda_random(float *x_gpu, size_t n) + static curandGenerator_t gen[16]; + static int init[16] = {0}; + +- request_gpu(); +- int i = cuda_get_device(); +- yield_gpu_with_remark("cuda_get_device"); ++ int i = cuda_get_device(); + + if(!init[i]){ +- request_gpu(); +- curandCreateGenerator(&gen[i], CURAND_RNG_PSEUDO_DEFAULT); +- yield_gpu_with_remark("curandCreateGenerator"); ++ curandCreateGenerator(&gen[i], CURAND_RNG_PSEUDO_DEFAULT); + +- request_gpu(); +- curandSetPseudoRandomGeneratorSeed(gen[i], time(0)); +- yield_gpu_with_remark("curandSetPseudoRandomGeneratorSeed"); ++ curandSetPseudoRandomGeneratorSeed(gen[i], time(0)); + + init[i] = 1; + } + +- request_gpu(); +- curandGenerateUniform(gen[i], x_gpu, n); +- yield_gpu_with_remark("curandGenerateUniform"); ++ curandGenerateUniform(gen[i], x_gpu, n); + + check_error(cudaPeekAtLastError()); + } +@@ -155,15 +143,11 @@ int *cuda_make_int_array(int *x, size_t n) + int *x_gpu; + size_t size = sizeof(int)*n; + +- request_gpu(); +- cudaError_t status = cudaMalloc((void **)&x_gpu, size); +- yield_gpu_with_remark("cudaMalloc"); ++ cudaError_t status = cudaMalloc((void **)&x_gpu, size); + + check_error(status); + if(x){ +- request_gpu(); +- status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); +- yield_gpu_with_remark("cuda_make_int_array"); ++ status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); + + check_error(status); + } +@@ -173,9 +157,7 @@ int *cuda_make_int_array(int *x, size_t n) + + void cuda_free(float *x_gpu) + { +- request_gpu(); +- cudaError_t status = cudaFree(x_gpu); +- yield_gpu_with_remark("free"); ++ cudaError_t status = cudaFree(x_gpu); + + check_error(status); + } +@@ -183,9 +165,7 @@ void cuda_free(float *x_gpu) + void cuda_push_array(float *x_gpu, float *x, size_t n) + { + size_t size = sizeof(float)*n; +- request_gpu(); +- cudaError_t status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); +- yield_gpu_with_remark("cuda_push_array"); ++ cudaError_t status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); + + check_error(status); + } +@@ -194,9 +174,7 @@ void cuda_pull_array(float *x_gpu, float *x, size_t n) + { + size_t size = sizeof(float)*n; + +- request_gpu(); +- cudaError_t status = cudaMemcpy(x, x_gpu, size, cudaMemcpyDeviceToHost); +- yield_gpu_with_remark("cuda_pull_array"); ++ cudaError_t status = cudaMemcpy(x, x_gpu, size, cudaMemcpyDeviceToHost); + + check_error(status); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu +index ad24b679..c6774fa7 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu +@@ -27,9 +27,7 @@ void forward_dropout_layer_gpu(dropout_layer layer, network net) + cuda_push_array(layer.rand_gpu, layer.rand, size); + */ + +- request_gpu(); +- yoloswag420blazeit360noscope<<>>(net.input_gpu, size, layer.rand_gpu, layer.probability, layer.scale); +- yield_gpu_with_remark("yoloswag420blazeit360noscope_forward"); ++ yoloswag420blazeit360noscope<<>>(net.input_gpu, size, layer.rand_gpu, layer.probability, layer.scale); + + check_error(cudaPeekAtLastError()); + } +@@ -39,9 +37,7 @@ void backward_dropout_layer_gpu(dropout_layer layer, network net) + if(!net.delta_gpu) return; + int size = layer.inputs*layer.batch; + +- request_gpu(); +- yoloswag420blazeit360noscope<<>>(net.delta_gpu, size, layer.rand_gpu, layer.probability, layer.scale); +- yield_gpu_with_remark("yoloswag420blazeit360noscope_backward"); ++ yoloswag420blazeit360noscope<<>>(net.delta_gpu, size, layer.rand_gpu, layer.probability, layer.scale); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu +index 0a73ec88..d2ccbdee 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu +@@ -54,11 +54,9 @@ void im2col_gpu(float *im, + int width_col = (width + 2 * pad - ksize) / stride + 1; + int num_kernels = channels * height_col * width_col; + +- request_gpu(); +- im2col_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK, ++ im2col_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK, + BLOCK>>>( + num_kernels, im, height, width, ksize, pad, + stride, height_col, + width_col, data_col); +- yield_gpu_with_remark("im2col_gpu_kernel"); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu +index 285ae322..12d920ee 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu +@@ -92,9 +92,7 @@ extern "C" void forward_maxpool_layer_gpu(maxpool_layer layer, network net) + + size_t n = h*w*c*layer.batch; + +- request_gpu(); +- forward_maxpool_layer_kernel<<>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, net.input_gpu, layer.output_gpu, layer.indexes_gpu); +- yield_gpu_with_remark("im2col_gpu_kernel"); ++ forward_maxpool_layer_kernel<<>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, net.input_gpu, layer.output_gpu, layer.indexes_gpu); + + check_error(cudaPeekAtLastError()); + } +@@ -103,9 +101,7 @@ extern "C" void backward_maxpool_layer_gpu(maxpool_layer layer, network net) + { + size_t n = layer.h*layer.w*layer.c*layer.batch; + +- request_gpu(); +- backward_maxpool_layer_kernel<<>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, layer.delta_gpu, net.delta_gpu, layer.indexes_gpu); +- yield_gpu_with_remark("im2col_gpu_kernel"); ++ backward_maxpool_layer_kernel<<>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, layer.delta_gpu, net.delta_gpu, layer.indexes_gpu); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp +index 133eaf33..7bc19835 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp +@@ -282,12 +282,6 @@ void Yolo3DetectorNode::image_callback(const sensor_msgs::ImageConstPtr& in_imag + + free(darknet_image_.data); + +- if(is_task_ready_ == TASK_NOT_READY){ +- init_task(); +- if(gpu_profiling_flag_) start_gpu_profiling(); +- } +- +- task_state_ = TASK_STATE_DONE; + } + + void Yolo3DetectorNode::config_cb(const autoware_config_msgs::ConfigSSD::ConstPtr& param) +@@ -327,52 +321,22 @@ void Yolo3DetectorNode::Run() + int key_id = 2; + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename_str; + int rate; + double task_minimum_inter_release_time; + double task_execution_time; + double task_relative_deadline; + +- int gpu_scheduling_flag; +- int gpu_profiling_flag; +- std::string gpu_execution_time_filename_str; +- std::string gpu_response_time_filename_str; +- std::string gpu_deadline_filename_str; +- +- private_node_handle.param("/vision_darknet_detect/task_scheduling_flag", task_scheduling_flag, 0); +- private_node_handle.param("/vision_darknet_detect/task_profiling_flag", task_profiling_flag, 0); + private_node_handle.param("/vision_darknet_detect/task_response_time_filename", task_response_time_filename_str, "~/Documents/profiling/response_time/vision_darknet_detect.csv"); + private_node_handle.param("/vision_darknet_detect/rate", rate, 10); + private_node_handle.param("/vision_darknet_detect/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + private_node_handle.param("/vision_darknet_detect/task_execution_time", task_execution_time, (double)10); + private_node_handle.param("/vision_darknet_detect/task_relative_deadline", task_relative_deadline, (double)10); +- private_node_handle.param("/vision_darknet_detect/gpu_scheduling_flag", gpu_scheduling_flag, 0); +- private_node_handle.param("/vision_darknet_detect/gpu_profiling_flag", gpu_profiling_flag, 0); +- private_node_handle.param("/vision_darknet_detect/gpu_execution_time_filename", gpu_execution_time_filename_str, "~/Documents/gpu_profiling/test_yolo_execution_time.csv"); +- private_node_handle.param("/vision_darknet_detect/gpu_response_time_filename", gpu_response_time_filename_str, "~/Documents/gpu_profiling/test_yolo_response_time.csv"); +- private_node_handle.param("/vision_darknet_detect/gpu_deadline_filename", gpu_deadline_filename_str, "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"); +- +- +- + + char* task_response_time_filename = strdup(task_response_time_filename_str.c_str()); +- char* gpu_execution_time_filename = strdup(gpu_execution_time_filename_str.c_str()); +- char* gpu_response_time_filename = strdup(gpu_response_time_filename_str.c_str()); +- char* gpu_deadline_filename = strdup(gpu_deadline_filename_str.c_str()); +- +- if(task_profiling_flag) init_task_profiling(task_response_time_filename); +- if(gpu_profiling_flag) init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename); +- +- if(gpu_scheduling_flag){ +- init_gpu_scheduling("/tmp/yolo", gpu_deadline_filename, key_id); +- }else if(gpu_scheduling_flag){ +- ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!"); +- exit(1); +- } +- +- ++ ++ init_task_profiling(task_response_time_filename); ++ + //RECEIVE IMAGE TOPIC NAME + std::string image_raw_topic_str; + if (private_node_handle.getParam("image_raw_node", image_raw_topic_str)) +@@ -449,39 +413,17 @@ void Yolo3DetectorNode::Run() + + ROS_INFO_STREAM( __APP_NAME__ << "" ); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(is_task_ready_) break; +- ros::spinOnce(); +- r.sleep(); +- } ++ ros::Rate r(rate); + +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) start_task_profiling(); +- if(task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- if(gpu_profiling_flag || gpu_scheduling_flag) start_job(); +- task_state_ = TASK_STATE_RUNNING; +- } ++ // Executing task ++ while(ros::ok()){ ++ start_task_profiling(); + +- ros::spinOnce(); ++ ros::spinOnce(); + +- if(task_profiling_flag) stop_task_profiling(0, task_state_); ++ stop_task_profiling(0, 0); + +- if(task_state_ == TASK_STATE_DONE){ +- if(gpu_profiling_flag || gpu_scheduling_flag) finish_job(); +- if(task_scheduling_flag) yield_task_scheduling(); +- task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ r.sleep(); + } + ROS_INFO("END Yolo"); + +diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp b/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp +index 67a65d23..9517d002 100644 +--- a/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp +@@ -240,7 +240,6 @@ void GlobalPlanner::callbackGetVehicleStatus(const geometry_msgs::TwistStampedCo + m_VehicleState.steer = atan(2.7 * msg->twist.angular.z/msg->twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleState.tStamp); + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); + } + + void GlobalPlanner::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) +@@ -447,8 +446,6 @@ void GlobalPlanner::MainLoop() + ros::NodeHandle private_nh("~"); + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; +@@ -458,8 +455,6 @@ void GlobalPlanner::MainLoop() + double multilap_replanning_distance; + int planning_fail_cnt; + +- private_nh.param("/op_global_planner/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_global_planner/task_profiling_flag", task_profiling_flag, 0); + private_nh.param("/op_global_planner/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_global_planner.csv"); + private_nh.param("/op_global_planner/rate", rate, 10); + private_nh.param("/op_global_planner/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +@@ -468,24 +463,16 @@ void GlobalPlanner::MainLoop() + private_nh.param("/op_global_planner/multilap_flag", multilap_flag, 0); + private_nh.param("/op_global_planner/multilap_replanning_distance", multilap_replanning_distance, (double)50); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + timespec animation_timer; + UtilityHNS::UtilityH::GetTickCount(animation_timer); + + ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25); +- +- + + while (ros::ok()) + { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ rubis::start_task_profiling(); + + ros::spinOnce(); + bool bMakeNewPlan = false; +@@ -618,14 +605,10 @@ void GlobalPlanner::MainLoop() + VisualizeDestinations(m_GoalsPos, m_iCurrentGoalIndex); + } + +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); ++ rubis::stop_task_profiling(0, 0); + +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } + loop_rate.sleep(); + } + } +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h +index 708301ff..ee2efa6d 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h +@@ -61,12 +61,17 @@ + #include + #include + ++#include "rubis_msgs/LaneArrayWithPoseTwist.h" ++#include "rubis_msgs/LaneWithPoseTwist.h" ++ + + namespace BehaviorGeneratorNS + { + + class BehaviorGen + { ++public: ++ const rubis_msgs::LaneArrayWithPoseTwist lane_array_with_pose_twist_msg_; + protected: //Planning Related variables + double PI = 3.14159265; + +@@ -120,12 +125,14 @@ protected: //Planning Related variables + double m_sprintSpeed; + bool m_sprintSwitch; + double m_obstacleWaitingTimeinIntersection; ++ double distance_to_pdestrian_; + + //ROS messages (topics) + ros::NodeHandle nh; + + //define publishers + ros::Publisher pub_LocalPath; ++ ros::Publisher pub_LocalPathWithPosePub; + ros::Publisher pub_LocalBasePath; + ros::Publisher pub_ClosestIndex; + ros::Publisher pub_BehaviorState; +@@ -159,15 +166,18 @@ protected: //Planning Related variables + ros::Subscriber sub_SprintSwitch; + ros::Subscriber sub_IntersectionCondition; + ++ // Others ++ timespec planningTimer; ++ std_msgs::Bool emergency_stop_msg; ++ + // Callback function for subscriber. +- void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); +- void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg); + void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); + void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg); + void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); +- void callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); ++ void callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg); + void callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstPtr& msg); + void callbackDistanceToPedestrian(const std_msgs::Float64& msg); ++ void _callbackDistanceToPedestrian(); + void callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg); + + void callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg); +@@ -179,7 +189,7 @@ protected: //Planning Related variables + + //Helper Functions + void UpdatePlanningParams(ros::NodeHandle& _nh); +- void SendLocalPlanningTopics(); ++ void SendLocalPlanningTopics(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg); + void VisualizeLocalPlanner(); + void LogLocalPlanningInfo(double dt); + bool GetBaseMapTF(); +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h +index 3890c27d..d1bf4bb6 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h +@@ -125,9 +125,12 @@ protected: + ros::Subscriber sub_can_info ; + ros::Subscriber sub_StepSignal; + ++ rubis_msgs::DetectedObjectArray objects_msgs_; ++ + // Callback function for subscriber. + void callbackGetTrackedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg); + void callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg); ++ void _callbackGetRubisTrackedObjects(rubis_msgs::DetectedObjectArray& objects_msg); + void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); + void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg); + void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h +index f2c4576d..a0324e44 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h +@@ -37,6 +37,7 @@ + #include + #include + #include ++#include "rubis_msgs/LaneArrayWithPoseTwist.h" + + #include "op_planner/PlannerCommonDef.h" + #include "op_planner/TrajectoryDynamicCosts.h" +@@ -50,6 +51,7 @@ namespace TrajectoryEvaluatorNS + class TrajectoryEval + { + protected: ++ bool is_objects_updated_; + + PlannerHNS::TrajectoryDynamicCosts m_TrajectoryCostsCalculator; + bool m_bUseMoveingObjectsPrediction; +@@ -101,6 +103,7 @@ protected: + ros::Publisher pub_CollisionPointsRviz; + ros::Publisher pub_LocalWeightedTrajectoriesRviz; + ros::Publisher pub_LocalWeightedTrajectories; ++ ros::Publisher pub_LocalWeightedTrajectoriesWithPoseTwist; + ros::Publisher pub_TrajectoryCost; + ros::Publisher pub_SafetyBorderRviz; + ros::Publisher pub_DistanceToPedestrian; +@@ -128,6 +131,9 @@ protected: + tf::StampedTransform m_velodyne_to_base_link; + tf::StampedTransform m_velodyne_to_map; + ++ // Others ++ std::vector intersection_list_; ++ autoware_msgs::DetectedObjectArray object_msg_; + + // Callback function for subscriber. + void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); +@@ -135,8 +141,9 @@ protected: + void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); + void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg); + void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); +- void callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); +- void callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg); ++ void callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg); ++ void callbackGetPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg); ++ void _callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArray& objects); + void callbackGetRubisPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg); + void callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr & msg); + void callbackGetCurrentState(const std_msgs::Int32 & msg); +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h +index f7dddce1..c47e5c2e 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h +@@ -30,12 +30,18 @@ + #include "op_planner/PlannerH.h" + #include "op_planner/PlannerCommonDef.h" + ++#include "rubis_msgs/PoseTwistStamped.h" ++#include "rubis_msgs/LaneArrayWithPoseTwist.h" ++ + namespace TrajectoryGeneratorNS + { + + class TrajectoryGen + { + protected: ++ geometry_msgs::PoseStamped current_pose_; ++ geometry_msgs::TwistStamped current_twist_; ++ + PlannerHNS::PlannerH m_Planner; + geometry_msgs::Pose m_OriginPos; + PlannerHNS::WayPoint m_InitPos; +@@ -59,11 +65,12 @@ protected: + PlannerHNS::CAR_BASIC_INFO m_CarInfo; + + +- //ROS messages (topics) ++ //ROS messages (topics) + ros::NodeHandle nh; + + //define publishers + ros::Publisher pub_LocalTrajectories; ++ ros::Publisher pub_LocalTrajectoriesWithPoseTwist; + ros::Publisher pub_LocalTrajectoriesRviz; + + // define subscribers. +@@ -73,16 +80,22 @@ protected: + ros::Subscriber sub_robot_odom; + ros::Subscriber sub_can_info; + ros::Subscriber sub_GlobalPlannerPaths; ++ ros::Subscriber sub_pose_twist; + ++ // Others ++ + + // Callback function for subscriber. + void callbackGetInitPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &input); +- void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); ++ // void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); ++ ++ void callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg); + void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg); + void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); + void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg); + void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); + ++ + //Helper Functions + void UpdatePlanningParams(ros::NodeHandle& _nh); + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch +index 0e794331..69cb1a60 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch +@@ -12,7 +12,7 @@ + + + +- ++ + + + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch +index 7899250e..7c0bf9ac 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch +@@ -8,7 +8,7 @@ + + + +- ++ + + + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch +index 37026b00..ef56b671 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch +@@ -1,6 +1,8 @@ + + + ++ ++ + + + +@@ -11,6 +13,10 @@ + + + ++ ++ ++ ++ + + + +@@ -25,7 +31,8 @@ + + + +- ++ ++ + + + +@@ -34,7 +41,11 @@ + + + +- ++ ++ ++ ++ ++ + + + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch +index 5b4c21ac..c1a16cfc 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch +@@ -1,10 +1,15 @@ + + ++ + + + + + ++ ++ ++ ++ + + + +@@ -12,11 +17,17 @@ + + + ++ ++ + + + + +- ++ ++ ++ ++ ++ + + + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp +index 9d824cfd..3c22305d 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp +@@ -34,6 +34,8 @@ BehaviorGen::BehaviorGen() + bBestCost = false; + bMap = false; + bRollOuts = false; ++ UtilityHNS::UtilityH::GetTickCount(planningTimer); ++ distance_to_pdestrian_ = 1000.0; + + ros::NodeHandle _nh; + UpdatePlanningParams(_nh); +@@ -50,6 +52,7 @@ BehaviorGen::BehaviorGen() + m_OriginPos.position.z = transform.getOrigin().z(); + + pub_LocalPath = nh.advertise("final_waypoints", 1,true); ++ pub_LocalPathWithPosePub = nh.advertise("final_waypoints_with_pose_twist", 1,true); + pub_LocalBasePath = nh.advertise("base_waypoints", 1,true); + pub_ClosestIndex = nh.advertise("closest_waypoint", 1,true); + pub_BehaviorState = nh.advertise("current_behavior", 1); +@@ -61,36 +64,19 @@ BehaviorGen::BehaviorGen() + pub_turnMarker = nh.advertise("turn_marker", 1); + pub_currentState = nh.advertise("current_state", 1); + +- sub_current_pose = nh.subscribe("/current_pose", 10, &BehaviorGen::callbackGetCurrentPose, this); +- +- int bVelSource = 1; +- _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); +- if(bVelSource == 0) +- sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this); +- else if(bVelSource == 1) +- sub_current_velocity = nh.subscribe("/current_velocity", 10, &BehaviorGen::callbackGetVehicleStatus, this); +- else if(bVelSource == 2) +- sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this); +- +- /* RT Scheduling setup */ +- // sub_current_pose = nh.subscribe("/current_pose", 1, &BehaviorGen::callbackGetCurrentPose, this); //origin 10 +- + // int bVelSource = 1; + // _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); + // if(bVelSource == 0) +- // sub_robot_odom = nh.subscribe("/odom", 1, &BehaviorGen::callbackGetRobotOdom, this); //origin 10 +- // else if(bVelSource == 1) +- // sub_current_velocity = nh.subscribe("/current_velocity", 1, &BehaviorGen::callbackGetVehicleStatus, this); //origin 10 ++ // sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this); + // else if(bVelSource == 2) +- // sub_can_info = nh.subscribe("/can_info", 1, &BehaviorGen::callbackGetCANInfo, this); //origin 10 ++ // sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this); + + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); +- sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories", 1, &BehaviorGen::callbackGetLocalPlannerPath, this); ++ sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories_with_pose_twist", 1, &BehaviorGen::callbackGetLocalPlannerPath, this); + // sub_TrafficLightStatus = nh.subscribe("/light_color", 1, &BehaviorGen::callbackGetTrafficLightStatus, this); + // sub_TrafficLightSignals = nh.subscribe("/roi_signal", 1, &BehaviorGen::callbackGetTrafficLightSignals, this); + sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this); +- +- sub_TrafficLightSignals = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this); ++ // sub_TrafficLightSignals = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this); + + sub_twist_raw = nh.subscribe("/twist_raw", 1, &BehaviorGen::callbackGetTwistRaw, this); + sub_twist_cmd = nh.subscribe("/twist_cmd", 1, &BehaviorGen::callbackGetTwistCMD, this); +@@ -206,9 +192,8 @@ void BehaviorGen::UpdatePlanningParams(ros::NodeHandle& _nh) + m_BehaviorGenerator.m_obstacleWaitingTimeinIntersection = m_obstacleWaitingTimeinIntersection; + } + +-void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ +- double distance = msg.data; +- if(distance < m_distanceToPedestrianThreshold){ ++void BehaviorGen::_callbackDistanceToPedestrian(){ ++ if(distance_to_pdestrian_ < m_distanceToPedestrianThreshold){ + m_PlanningParams.pedestrianAppearence = true; + } + else +@@ -216,7 +201,10 @@ void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ + m_PlanningParams.pedestrianAppearence = false; + } + m_BehaviorGenerator.UpdatePedestrianAppearence(m_PlanningParams.pedestrianAppearence); +- // m_BehaviorGenerator.printPedestrianAppearence(); ++} ++ ++void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ ++ distance_to_pdestrian_ = msg.data; + } + + void BehaviorGen::callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg){ +@@ -245,24 +233,6 @@ void BehaviorGen::callbackGetCommandCMD(const autoware_msgs::ControlCommandConst + m_Ctrl_cmd = *msg; + } + +-void BehaviorGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +-{ +- m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); +- bNewCurrentPos = true; +-} +- +-void BehaviorGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) +-{ +- m_VehicleStatus.speed = msg->twist.linear.x; +- m_CurrentPos.v = m_VehicleStatus.speed; +- if(fabs(msg->twist.linear.x) > 0.25) +- m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); +- UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); +- bVehicleStatus = true; +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +-} +- + void BehaviorGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) + { + m_VehicleStatus.speed = msg->speed/3.6; +@@ -373,17 +343,46 @@ void BehaviorGen::callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstP + m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity; + } + +-void BehaviorGen::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++void BehaviorGen::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) + { +- if(msg->lanes.size() > 0) ++ // Before spinOnce ++ rubis::start_task_profiling(); ++ rubis::instance_ = msg->instance; ++ rubis::obj_instance_ = msg->obj_instance; ++ ++ // Callback for distance to pedestrian ++ _callbackDistanceToPedestrian(); ++ ++ static double prev_x = 0.0, prev_y = 0.0, prev_speed = 0.0; ++ // Callback for current velocity ++ if(prev_speed != msg->twist.twist.linear.x){ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ prev_speed = msg->twist.twist.linear.x; ++ } ++ ++ // Callback for current pose ++ if(prev_x != msg->pose.pose.position.x || prev_y != msg->pose.pose.position.y){ ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ bNewCurrentPos = true; ++ prev_x = msg->pose.pose.position.x; ++ prev_y = msg->pose.pose.position.y; ++ } ++ ++ // Callback for local planner path ++ if(msg->lane_array.lanes.size() > 0) + { + m_RollOuts.clear(); + int globalPathId_roll_outs = -1; + +- for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) + { + std::vector path; +- PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), path); ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); + m_RollOuts.push_back(path); + + if(path.size() > 0) +@@ -410,6 +409,143 @@ void BehaviorGen::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayCons + m_BehaviorGenerator.m_RollOuts = m_RollOuts; + bRollOuts = true; + } ++ ++ // Main Loop ++ // Check Pedestrian is Appeared ++ double dt = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer); ++ UtilityHNS::UtilityH::GetTickCount(planningTimer); ++ ++ if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); ++ } ++ else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); ++ ++ } ++ else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) ++ { ++ std::vector conn_data;; ++ ++ if(m_MapRaw.GetVersion()==2) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, ++ m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); ++ ++ try{ ++ // Add Traffic Signal Info from yaml file ++ XmlRpc::XmlRpcValue traffic_light_list; ++ nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list); ++ ++ // Add Stop Line Info from yaml file ++ XmlRpc::XmlRpcValue stop_line_list; ++ nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list); ++ ++ // Add Crossing Info from yaml file ++ // XmlRpc::XmlRpcValue intersection_list; ++ // nh.getParam("/op_behavior_selector/intersection_list", intersection_list); ++ ++ PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list); ++ } ++ catch(XmlRpc::XmlRpcException& e){ ++ ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str()); ++ exit(1); ++ } ++ ++ m_BehaviorGenerator.m_Map = m_Map; ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl; ++ } ++ } ++ else if(m_MapRaw.GetVersion()==1) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl; ++ } ++ } ++ } ++ ++ if(bNewCurrentPos && m_GlobalPaths.size()>0) ++ { ++ if(bNewLightSignal) ++ { ++ m_PrevTrafficLight = m_CurrTrafficLight; ++ bNewLightSignal = false; ++ } ++ ++ if(bNewLightStatus) ++ { ++ bNewLightStatus = false; ++ for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++) ++ m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus; ++ } ++ ++ m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch; ++ m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0); ++ std_msgs::Int32 curr_state_msg; ++ curr_state_msg.data = m_CurrentBehavior.state; ++ ++ pub_currentState.publish(curr_state_msg); ++ ++ CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint); ++ m_BehaviorGenerator.m_turnAngle = m_turnAngle; ++ ++ std_msgs::Float64 turn_angle_msg; ++ turn_angle_msg.data = m_turnAngle; ++ pub_turnAngle.publish(turn_angle_msg); ++ ++ emergency_stop_msg.data = false; ++ if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop! ++ emergency_stop_msg.data = true; ++ pub_EmergencyStop.publish(emergency_stop_msg); ++ ++ SendLocalPlanningTopics(msg); ++ VisualizeLocalPlanner(); ++ LogLocalPlanningInfo(dt); ++ ++ // Publish turn_marker ++ visualization_msgs::MarkerArray turn_marker; ++ visualization_msgs::Marker marker; ++ marker.header.frame_id = "map"; ++ marker.type = 2; ++ marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x; ++ marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y; ++ marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z; ++ marker.scale.x = 3; ++ marker.scale.y = 3; ++ marker.scale.z = 3; ++ marker.color.r = 0.0f; ++ marker.color.g = 1.0f; ++ marker.color.b = 0.0f; ++ marker.color.a = 1.0f; ++ marker.header.stamp = ros::Time::now(); ++ marker.header.frame_id = "map"; ++ turn_marker.markers.push_back(marker); ++ ++ pub_turnMarker.publish(turn_marker); ++ } ++ else ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + void BehaviorGen::callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg) +@@ -481,7 +617,7 @@ void BehaviorGen::VisualizeLocalPlanner() + // pub_SelectedPathRviz.publish(selected_path); + } + +-void BehaviorGen::SendLocalPlanningTopics() ++void BehaviorGen::SendLocalPlanningTopics(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) + { + //Send Behavior State + geometry_msgs::Twist t; +@@ -523,13 +659,23 @@ void BehaviorGen::SendLocalPlanningTopics() + PlannerHNS::RelativeInfo info; + PlannerHNS::PlanningHelpers::GetRelativeInfo(m_BehaviorGenerator.m_Path, m_BehaviorGenerator.state, info); + PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_BehaviorGenerator.m_Path, m_CurrentTrajectoryToSend, info.iBack); +- //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl; ++ //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl; + + closest_waypoint.data = 1; + pub_ClosestIndex.publish(closest_waypoint); + pub_LocalBasePath.publish(m_CurrentTrajectoryToSend); ++ ++ rubis_msgs::LaneWithPoseTwist final_waypoints_with_pose_twist_msg; ++ final_waypoints_with_pose_twist_msg.instance = rubis::instance_; ++ final_waypoints_with_pose_twist_msg.obj_instance = rubis::obj_instance_; ++ final_waypoints_with_pose_twist_msg.lane = m_CurrentTrajectoryToSend; ++ final_waypoints_with_pose_twist_msg.pose = msg->pose; ++ final_waypoints_with_pose_twist_msg.twist = msg->twist; ++ ++ pub_LocalPathWithPosePub.publish(final_waypoints_with_pose_twist_msg); + pub_LocalPath.publish(m_CurrentTrajectoryToSend); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ ++ + } + + void BehaviorGen::LogLocalPlanningInfo(double dt) +@@ -606,195 +752,32 @@ void BehaviorGen::MainLoop() + { + ros::NodeHandle private_nh("~"); + +- timespec planningTimer; +- UtilityHNS::UtilityH::GetTickCount(planningTimer); +- std_msgs::Bool emergency_stop_msg; +- +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- + m_BehaviorGenerator.m_turnThreshold = m_turnThreshold; +- +-m_sprintSwitch = false; +- +- private_nh.param("/op_behavior_selector/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_behavior_selector/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/op_behavior_selector/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv"); +- private_nh.param("/op_behavior_selector/rate", rate, 10); +- private_nh.param("/op_behavior_selector/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/op_behavior_selector/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/op_behavior_selector/task_relative_deadline", task_relative_deadline, (double)10); +- +- /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25); +- +- struct timespec start_time, end_time; +- + m_sprintSwitch = false; + +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- // Check Pedestrian is Appeared +- double dt = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer); +- UtilityHNS::UtilityH::GetTickCount(planningTimer); +- +- if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) +- { +- bMap = true; +- PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); +- } +- else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) +- { +- bMap = true; +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); +- +- } +- else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) +- { +- std::vector conn_data;; +- +- if(m_MapRaw.GetVersion()==2) +- { +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, +- m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, +- m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, +- m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, +- m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, +- m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); +- +- try{ +- // Add Traffic Signal Info from yaml file +- XmlRpc::XmlRpcValue traffic_light_list; +- nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list); +- +- // Add Stop Line Info from yaml file +- XmlRpc::XmlRpcValue stop_line_list; +- nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list); +- +- // Add Crossing Info from yaml file +- // XmlRpc::XmlRpcValue intersection_list; +- // nh.getParam("/op_behavior_selector/intersection_list", intersection_list); +- +- PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list); +- } +- catch(XmlRpc::XmlRpcException& e){ +- ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str()); +- exit(1); +- } +- +- m_BehaviorGenerator.m_Map = m_Map; +- +- if(m_Map.roadSegments.size() > 0) +- { +- bMap = true; +- std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl; +- } +- } +- else if(m_MapRaw.GetVersion()==1) +- { +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, +- m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, +- m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, +- m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, +- m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); +- +- if(m_Map.roadSegments.size() > 0) +- { +- bMap = true; +- std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl; +- } +- } +- } +- +- if(bNewCurrentPos && m_GlobalPaths.size()>0) +- { +- if(bNewLightSignal) +- { +- m_PrevTrafficLight = m_CurrTrafficLight; +- bNewLightSignal = false; +- } +- +- if(bNewLightStatus) +- { +- bNewLightStatus = false; +- for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++) +- m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus; +- } +- +- m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch; +- m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0); +- std_msgs::Int32 curr_state_msg; +- curr_state_msg.data = m_CurrentBehavior.state; +- +- pub_currentState.publish(curr_state_msg); +- +- CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint); +- m_BehaviorGenerator.m_turnAngle = m_turnAngle; +- +- std_msgs::Float64 turn_angle_msg; +- turn_angle_msg.data = m_turnAngle; +- pub_turnAngle.publish(turn_angle_msg); +- +- emergency_stop_msg.data = false; +- if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop! +- emergency_stop_msg.data = true; +- pub_EmergencyStop.publish(emergency_stop_msg); +- +- SendLocalPlanningTopics(); +- VisualizeLocalPlanner(); +- LogLocalPlanningInfo(dt); +- +- // Publish turn_marker +- visualization_msgs::MarkerArray turn_marker; +- visualization_msgs::Marker marker; +- marker.header.frame_id = "map"; +- marker.type = 2; +- marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x; +- marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y; +- marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z; +- marker.scale.x = 3; +- marker.scale.y = 3; +- marker.scale.z = 3; +- marker.color.r = 0.0f; +- marker.color.g = 1.0f; +- marker.color.b = 0.0f; +- marker.color.a = 1.0f; +- marker.header.stamp = ros::Time::now(); +- marker.header.frame_id = "map"; +- turn_marker.markers.push_back(marker); +- +- pub_turnMarker.publish(turn_marker); +- } +- else +- sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); ++ std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv"); + +- loop_rate.sleep(); +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + } + + bool BehaviorGen::GetBaseMapTF(){ +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp +new file mode 100644 +index 00000000..250edb07 +--- /dev/null ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp +@@ -0,0 +1,905 @@ ++/* ++ * Copyright 2018-2019 Autoware Foundation. All rights reserved. ++ * ++ * Licensed under the Apache License, Version 2.0 (the "License"); ++ * you may not use this file except in compliance with the License. ++ * You may obtain a copy of the License at ++ * ++ * http://www.apache.org/licenses/LICENSE-2.0 ++ * ++ * Unless required by applicable law or agreed to in writing, software ++ * distributed under the License is distributed on an "AS IS" BASIS, ++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ++ * See the License for the specific language governing permissions and ++ * limitations under the License. ++ */ ++ ++#include "op_behavior_selector_core.h" ++#include "op_ros_helpers/op_ROSHelpers.h" ++#include "op_planner/MappingHelpers.h" ++#include ++ ++namespace BehaviorGeneratorNS ++{ ++ ++BehaviorGen::BehaviorGen() ++{ ++ sleep(2); ++ bNewCurrentPos = false; ++ bVehicleStatus = false; ++ bWayGlobalPath = false; ++ bWayGlobalPathLogs = false; ++ bNewLightStatus = false; ++ bNewLightSignal = false; ++ bBestCost = false; ++ bMap = false; ++ bRollOuts = false; ++ UtilityHNS::UtilityH::GetTickCount(planningTimer); ++ ++ ros::NodeHandle _nh; ++ UpdatePlanningParams(_nh); ++ ++ // RUBIS driving parameter ++ nh.getParam("/op_behavior_selector/distanceToPedestrianThreshold", m_distanceToPedestrianThreshold); ++ nh.param("/op_behavior_selector/turnThreshold", m_turnThreshold, 20.0); ++ ++ ++ tf::StampedTransform transform; ++ PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); ++ m_OriginPos.position.x = transform.getOrigin().x(); ++ m_OriginPos.position.y = transform.getOrigin().y(); ++ m_OriginPos.position.z = transform.getOrigin().z(); ++ ++ pub_LocalPath = nh.advertise("final_waypoints", 1,true); ++ pub_LocalPathWithPosePub = nh.advertise("final_waypoints_with_pose_twist", 1,true); ++ pub_LocalBasePath = nh.advertise("base_waypoints", 1,true); ++ pub_ClosestIndex = nh.advertise("closest_waypoint", 1,true); ++ pub_BehaviorState = nh.advertise("current_behavior", 1); ++ pub_SimuBoxPose = nh.advertise("sim_box_pose_ego", 1); ++ pub_BehaviorStateRviz = nh.advertise("behavior_state", 1); ++ pub_SelectedPathRviz = nh.advertise("local_selected_trajectory_rviz", 1); ++ pub_EmergencyStop = nh.advertise("emergency_stop", 1); ++ pub_turnAngle = nh.advertise("turn_angle", 1); ++ pub_turnMarker = nh.advertise("turn_marker", 1); ++ pub_currentState = nh.advertise("current_state", 1); ++ ++ int bVelSource = 1; ++ _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); ++ if(bVelSource == 0) ++ sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this); ++ else if(bVelSource == 2) ++ sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this); ++ ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); ++ sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories_with_pose_twist", 1, &BehaviorGen::callbackGetLocalPlannerPath, this); ++ // sub_TrafficLightStatus = nh.subscribe("/light_color", 1, &BehaviorGen::callbackGetTrafficLightStatus, this); ++ // sub_TrafficLightSignals = nh.subscribe("/roi_signal", 1, &BehaviorGen::callbackGetTrafficLightSignals, this); ++ sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this); ++ ++ sub_TrafficLightSignals = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this); ++ ++ sub_twist_raw = nh.subscribe("/twist_raw", 1, &BehaviorGen::callbackGetTwistRaw, this); ++ sub_twist_cmd = nh.subscribe("/twist_cmd", 1, &BehaviorGen::callbackGetTwistCMD, this); ++ //sub_ctrl_cmd = nh.subscribe("/ctrl_cmd", 1, &BehaviorGen::callbackGetCommandCMD, this); ++ sub_DistanceToPedestrian = nh.subscribe("/distance_to_pedestrian", 1, &BehaviorGen::callbackDistanceToPedestrian, this); ++ sub_IntersectionCondition = nh.subscribe("/intersection_condition", 1, &BehaviorGen::callbackIntersectionCondition, this); ++ sub_SprintSwitch = nh.subscribe("/sprint_switch", 1, &BehaviorGen::callbackSprintSwitch, this); ++ ++ //Mapping Section ++ sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &BehaviorGen::callbackGetVMLanes, this); ++ sub_points = nh.subscribe("/vector_map_info/point", 1, &BehaviorGen::callbackGetVMPoints, this); ++ sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &BehaviorGen::callbackGetVMdtLanes, this); ++ sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &BehaviorGen::callbackGetVMIntersections, this); ++ sup_area = nh.subscribe("/vector_map_info/area", 1, &BehaviorGen::callbackGetVMAreas, this); ++ sub_lines = nh.subscribe("/vector_map_info/line", 1, &BehaviorGen::callbackGetVMLines, this); ++ sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &BehaviorGen::callbackGetVMStopLines, this); ++ sub_signals = nh.subscribe("/vector_map_info/signal", 1, &BehaviorGen::callbackGetVMSignal, this); ++ sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &BehaviorGen::callbackGetVMVectors, this); ++ sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &BehaviorGen::callbackGetVMCurbs, this); ++ sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &BehaviorGen::callbackGetVMRoadEdges, this); ++ sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &BehaviorGen::callbackGetVMWayAreas, this); ++ sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &BehaviorGen::callbackGetVMCrossWalks, this); ++ sub_nodes = nh.subscribe("/vector_map_info/node", 1, &BehaviorGen::callbackGetVMNodes, this); ++} ++ ++BehaviorGen::~BehaviorGen() ++{ ++ UtilityHNS::DataRW::WriteLogData(UtilityHNS::UtilityH::GetHomeDirectory()+UtilityHNS::DataRW::LoggingMainfolderName+UtilityHNS::DataRW::StatesLogFolderName, "MainLog", ++ "time,dt, Behavior_i, Behavior_str, RollOuts_n, Blocked_i, Central_i, Selected_i, StopSign_id, Light_id, Stop_Dist, Follow_Dist, Follow_Vel," ++ "Target_Vel, PID_Vel, T_cmd_Vel, C_cmd_Vel, Vel, Steer, X, Y, Z, Theta," ++ , m_LogData); ++} ++ ++void BehaviorGen::UpdatePlanningParams(ros::NodeHandle& _nh) ++{ ++ _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); ++ if(m_PlanningParams.enableSwerving) ++ m_PlanningParams.enableFollowing = true; ++ else ++ _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); ++ ++ _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); ++ _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); ++ ++ _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); ++ _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); ++ _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); ++ ++ _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); ++ ++ _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); ++ if(m_PlanningParams.enableSwerving) ++ _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); ++ else ++ m_PlanningParams.rollOutNumber = 0; ++ ++ _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); ++ _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); ++ _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); ++ _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); ++ _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); ++ ++ _nh.getParam("/op_trajectory_evaluator/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); ++ _nh.getParam("/op_trajectory_evaluator/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); ++ ++ _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); ++ ++ _nh.getParam("/op_common_params/width", m_CarInfo.width); ++ _nh.getParam("/op_common_params/length", m_CarInfo.length); ++ _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); ++ _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); ++ _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); ++ _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); ++ _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); ++ m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; ++ m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; ++ ++ _nh.getParam("/op_common_params/stopLineMargin", m_PlanningParams.stopLineMargin); ++ _nh.getParam("/op_common_params/stopLineDetectionDistance", m_PlanningParams.stopLineDetectionDistance); ++ ++ PlannerHNS::ControllerParams controlParams; ++ controlParams.Steering_Gain = PlannerHNS::PID_CONST(0.07, 0.02, 0.01); ++ controlParams.Velocity_Gain = PlannerHNS::PID_CONST(0.1, 0.005, 0.1); ++ nh.getParam("/op_common_params/steeringDelay", controlParams.SteeringDelay); ++ nh.getParam("/op_common_params/minPursuiteDistance", controlParams.minPursuiteDistance ); ++ nh.getParam("/op_common_params/additionalBrakingDistance", m_PlanningParams.additionalBrakingDistance ); ++ nh.getParam("/op_common_params/giveUpDistance", m_PlanningParams.giveUpDistance ); ++ nh.getParam("/op_common_params/enableSlowDownOnCurve", m_PlanningParams.enableSlowDownOnCurve ); ++ nh.getParam("/op_common_params/curveVelocityRatio", m_PlanningParams.curveVelocityRatio ); ++ ++ int iSource = 0; ++ _nh.getParam("/op_common_params/mapSource" , iSource); ++ if(iSource == 0) ++ m_MapType = PlannerHNS::MAP_AUTOWARE; ++ else if (iSource == 1) ++ m_MapType = PlannerHNS::MAP_FOLDER; ++ else if(iSource == 2) ++ m_MapType = PlannerHNS::MAP_KML_FILE; ++ ++ _nh.getParam("/op_common_params/mapFileName" , m_MapPath); ++ ++ _nh.getParam("/op_behavior_selector/evidence_tust_number", m_PlanningParams.nReliableCount); ++ ++ //std::cout << "nReliableCount: " << m_PlanningParams.nReliableCount << std::endl; ++ ++ _nh.param("/op_behavior_selector/sprintSpeed", m_sprintSpeed, 13.5); ++ _nh.param("/op_behavior_selector/obstacleWaitingTimeinIntersection", m_obstacleWaitingTimeinIntersection, 1.0); ++ ++ m_BehaviorGenerator.Init(controlParams, m_PlanningParams, m_CarInfo, m_sprintSpeed); ++ ++ m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior = PlannerHNS::INITIAL_STATE; ++ ++ m_BehaviorGenerator.m_obstacleWaitingTimeinIntersection = m_obstacleWaitingTimeinIntersection; ++} ++ ++void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ ++ double distance = msg.data; ++ if(distance < m_distanceToPedestrianThreshold){ ++ m_PlanningParams.pedestrianAppearence = true; ++ } ++ else ++ { ++ m_PlanningParams.pedestrianAppearence = false; ++ } ++ m_BehaviorGenerator.UpdatePedestrianAppearence(m_PlanningParams.pedestrianAppearence); ++ // m_BehaviorGenerator.printPedestrianAppearence(); ++} ++ ++void BehaviorGen::callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg){ ++ m_BehaviorGenerator.m_isInsideIntersection = msg.isIntersection; ++ m_BehaviorGenerator.m_closestIntersectionDistance = msg.intersectionDistance; ++ m_BehaviorGenerator.m_riskyLeft = msg.riskyLeftTurn; ++ m_BehaviorGenerator.m_riskyRight = msg.riskyRightTurn; ++} ++ ++void BehaviorGen::callbackSprintSwitch(const std_msgs::Bool& msg){ ++ m_sprintSwitch = msg.data; ++} ++ ++void BehaviorGen::callbackGetTwistRaw(const geometry_msgs::TwistStampedConstPtr& msg) ++{ ++ m_Twist_raw = *msg; ++} ++ ++void BehaviorGen::callbackGetTwistCMD(const geometry_msgs::TwistStampedConstPtr& msg) ++{ ++ m_Twist_cmd = *msg; ++} ++ ++void BehaviorGen::callbackGetCommandCMD(const autoware_msgs::ControlCommandConstPtr& msg) ++{ ++ m_Ctrl_cmd = *msg; ++} ++ ++void BehaviorGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) ++{ ++ m_VehicleStatus.speed = msg->speed/3.6; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void BehaviorGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) ++{ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed ; ++ if(msg->twist.twist.linear.x != 0) ++ m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void BehaviorGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++{ ++ if(msg->lanes.size() > 0 && bMap) ++ { ++ ++ bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); ++ ++ m_GlobalPaths.clear(); ++ ++ for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ { ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); ++ PlannerHNS::Lane* pPrevValid = 0; ++ for(unsigned int j = 0 ; j < m_temp_path.size(); j++) ++ { ++ PlannerHNS::Lane* pLane = 0; ++ pLane = PlannerHNS::MappingHelpers::GetLaneById(m_temp_path.at(j).laneId, m_Map); ++ if(!pLane) ++ { ++ pLane = PlannerHNS::MappingHelpers::GetClosestLaneFromMapDirectionBased(m_temp_path.at(j), m_Map, 1); ++ ++ if(!pLane && !pPrevValid) ++ { ++ ROS_ERROR("Map inconsistency between Global Path and Local Planer Map, Can't identify current lane."); ++ return; ++ } ++ ++ if(!pLane) ++ m_temp_path.at(j).pLane = pPrevValid; ++ else ++ { ++ m_temp_path.at(j).pLane = pLane; ++ pPrevValid = pLane ; ++ } ++ ++ m_temp_path.at(j).laneId = m_temp_path.at(j).pLane->id; ++ } ++ else ++ m_temp_path.at(j).pLane = pLane; ++ ++ ++ //std::cout << "StopLineInGlobalPath: " << m_temp_path.at(j).stopLineID << std::endl; ++ } ++ ++ PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); ++ m_GlobalPaths.push_back(m_temp_path); ++ ++ if(bOldGlobalPath) ++ { ++ bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); ++ } ++ } ++ ++ if(!bOldGlobalPath) ++ { ++ bWayGlobalPath = true; ++ bWayGlobalPathLogs = true; ++ for(unsigned int i = 0; i < m_GlobalPaths.size(); i++) ++ { ++ PlannerHNS::PlanningHelpers::FixPathDensity(m_GlobalPaths.at(i), m_PlanningParams.pathDensity); ++ PlannerHNS::PlanningHelpers::SmoothPath(m_GlobalPaths.at(i), 0.35, 0.4, 0.05); ++ ++ PlannerHNS::PlanningHelpers::GenerateRecommendedSpeed(m_GlobalPaths.at(i), m_CarInfo.max_speed_forward, m_PlanningParams.speedProfileFactor); ++ m_GlobalPaths.at(i).at(m_GlobalPaths.at(i).size()-1).v = 0; ++ } ++ ++ std::cout << "Received New Global Path Selector! " << std::endl; ++ } ++ else ++ { ++ m_GlobalPaths.clear(); ++ } ++ } ++} ++ ++void BehaviorGen::callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstPtr& msg) ++{ ++ if(m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior == PlannerHNS::INTERSECTION_STATE){ ++ bBestCost = true; ++ m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance; ++ m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity; ++ return; ++ } ++ bBestCost = true; ++ m_TrajectoryBestCost.bBlocked = msg->is_blocked; ++ m_TrajectoryBestCost.index = msg->lane_index; ++ m_TrajectoryBestCost.cost = msg->cost; ++ m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance; ++ m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity; ++} ++ ++void BehaviorGen::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) ++{ ++ rubis::instance_ = msg->instance; ++ lane_array_with_pose_twist_msg_ = msg; ++ ++ // Callback ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ bNewCurrentPos = true; ++ ++ if(msg->lane_array.lanes.size() > 0) ++ { ++ m_RollOuts.clear(); ++ int globalPathId_roll_outs = -1; ++ ++ for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) ++ { ++ std::vector path; ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); ++ m_RollOuts.push_back(path); ++ ++ if(path.size() > 0) ++ globalPathId_roll_outs = path.at(0).gid; ++ } ++ ++ if(bWayGlobalPath && m_GlobalPaths.size() > 0) ++ { ++ if(m_GlobalPaths.at(0).size() > 0) ++ { ++ int globalPathId = m_GlobalPaths.at(0).at(0).gid; ++ std::cout << "Before Synchronization At Behavior Selector: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; ++ ++ if(globalPathId_roll_outs == globalPathId) ++ { ++ bWayGlobalPath = false; ++ m_GlobalPathsToUse = m_GlobalPaths; ++ m_BehaviorGenerator.SetNewGlobalPath(m_GlobalPathsToUse); ++ std::cout << "Synchronization At Behavior Selector: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; ++ } ++ } ++ } ++ ++ m_BehaviorGenerator.m_RollOuts = m_RollOuts; ++ bRollOuts = true; ++ } ++} ++ ++void BehaviorGen::callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg) ++{ ++ bNewLightSignal = true; ++ std::vector simulatedLights; ++ for(unsigned int i = 0 ; i < msg.signals.size() ; i++) ++ { ++ PlannerHNS::TrafficLight tl; ++ tl.id = msg.signals.at(i).id; ++ tl.remainTime = msg.signals.at(i).time; ++ ++ for(unsigned int k = 0; k < m_Map.trafficLights.size(); k++) ++ { ++ if(m_Map.trafficLights.at(k).id == tl.id) ++ { ++ tl.pos = m_Map.trafficLights.at(k).pos; ++ tl.routine = m_Map.trafficLights.at(k).routine; ++ break; ++ } ++ } ++ ++ if(msg.signals.at(i).type == 0) ++ { ++ tl.lightState = PlannerHNS::RED_LIGHT; ++ } ++ else if(msg.signals.at(i).type == 1) ++ { ++ tl.lightState = PlannerHNS::YELLOW_LIGHT; ++ } ++ else ++ { ++ tl.lightState = PlannerHNS::GREEN_LIGHT; ++ } ++ ++ simulatedLights.push_back(tl); ++ } ++ ++ m_CurrTrafficLight = simulatedLights; ++} ++ ++void BehaviorGen::VisualizeLocalPlanner() ++{ ++ visualization_msgs::Marker behavior_rviz; ++ int iDirection = 0; ++ if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory > m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory) ++ iDirection = 1; ++ else if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory < m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory) ++ iDirection = -1; ++ PlannerHNS::ROSHelpers::VisualizeBehaviorState(m_CurrentPos, m_CurrentBehavior, !m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bTrafficIsRed , iDirection, behavior_rviz, "beh_state"); ++ //pub_BehaviorStateRviz.publish(behavior_rviz); ++ ++ visualization_msgs::MarkerArray markerArray; ++ ++ //PlannerHNS::ROSHelpers::GetIndicatorArrows(m_CurrentPos, m_CarInfo.width, m_CarInfo.length, m_CurrentBehavior.indicator, 0, markerArray); ++ ++ markerArray.markers.push_back(behavior_rviz); ++ ++ pub_BehaviorStateRviz.publish(markerArray); ++ ++ //To Test Synchronization Problem ++// visualization_msgs::MarkerArray selected_path; ++// std::vector > > paths; ++// paths.push_back(std::vector >()); ++// paths.at(0).push_back(m_BehaviorGenerator.m_Path); ++// paths.push_back(m_GlobalPathsToUse); ++// paths.push_back(m_RollOuts); ++// PlannerHNS::ROSHelpers::TrajectoriesToMarkers(paths, selected_path); ++// pub_SelectedPathRviz.publish(selected_path); ++} ++ ++void BehaviorGen::SendLocalPlanningTopics(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) ++{ ++ //Send Behavior State ++ geometry_msgs::Twist t; ++ geometry_msgs::TwistStamped behavior; ++ t.linear.x = m_CurrentBehavior.bNewPlan; ++ t.linear.y = m_CurrentBehavior.followDistance; ++ t.linear.z = m_CurrentBehavior.followVelocity; ++ t.angular.x = (int)m_CurrentBehavior.indicator; ++ t.angular.y = (int)m_CurrentBehavior.state; ++ t.angular.z = m_CurrentBehavior.iTrajectory; ++ behavior.twist = t; ++ behavior.header.stamp = ros::Time::now(); ++ pub_BehaviorState.publish(behavior); ++ ++ //Send Ego Vehicle Simulation Pose Data ++ geometry_msgs::PoseArray sim_data; ++ geometry_msgs::Pose p_id, p_pose, p_box; ++ ++ sim_data.header.frame_id = "map"; ++ sim_data.header.stamp = ros::Time(); ++ p_id.position.x = 0; ++ p_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a)); ++ ++ PlannerHNS::WayPoint pose_center = PlannerHNS::PlanningHelpers::GetRealCenter(m_BehaviorGenerator.state, m_CarInfo.wheel_base); ++ ++ p_pose.position.x = pose_center.pos.x; ++ p_pose.position.y = pose_center.pos.y; ++ p_pose.position.z = pose_center.pos.z; ++ p_box.position.x = m_BehaviorGenerator.m_CarInfo.width; ++ p_box.position.y = m_BehaviorGenerator.m_CarInfo.length; ++ p_box.position.z = 2.2; ++ sim_data.poses.push_back(p_id); ++ sim_data.poses.push_back(p_pose); ++ sim_data.poses.push_back(p_box); ++ pub_SimuBoxPose.publish(sim_data); ++ ++ //Send Trajectory Data to path following nodes ++ std_msgs::Int32 closest_waypoint; ++ PlannerHNS::RelativeInfo info; ++ PlannerHNS::PlanningHelpers::GetRelativeInfo(m_BehaviorGenerator.m_Path, m_BehaviorGenerator.state, info); ++ PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_BehaviorGenerator.m_Path, m_CurrentTrajectoryToSend, info.iBack); ++ //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl; ++ ++ closest_waypoint.data = 1; ++ pub_ClosestIndex.publish(closest_waypoint); ++ pub_LocalBasePath.publish(m_CurrentTrajectoryToSend); ++ ++ rubis_msgs::LaneWithPoseTwist final_waypoints_with_pose_twist_msg; ++ final_waypoints_with_pose_twist_msg.instance = rubis::instance_; ++ final_waypoints_with_pose_twist_msg.lane = m_CurrentTrajectoryToSend; ++ final_waypoints_with_pose_twist_msg.pose = msg->pose; ++ final_waypoints_with_pose_twist_msg.twist = msg->twist; ++ ++ pub_LocalPathWithPosePub.publish(final_waypoints_with_pose_twist_msg); ++ pub_LocalPath.publish(m_CurrentTrajectoryToSend); ++ ++ ++} ++ ++void BehaviorGen::LogLocalPlanningInfo(double dt) ++{ ++ timespec log_t; ++ UtilityHNS::UtilityH::GetTickCount(log_t); ++ std::ostringstream dataLine; ++ dataLine << UtilityHNS::UtilityH::GetLongTime(log_t) <<"," << dt << "," << m_CurrentBehavior.state << ","<< PlannerHNS::ROSHelpers::GetBehaviorNameFromCode(m_CurrentBehavior.state) << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->m_pParams->rollOutNumber << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bFullyBlock << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentStopSignID << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentTrafficLightID << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->minStoppingDistance << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->distanceToNext << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->velocityOfNext << "," << ++ m_CurrentBehavior.maxVelocity << "," << ++ m_Twist_raw.twist.linear.x << "," << ++ m_Twist_cmd.twist.linear.x << "," << ++ m_Ctrl_cmd.linear_velocity << "," << ++ m_VehicleStatus.speed << "," << ++ m_VehicleStatus.steer << "," << ++ m_BehaviorGenerator.state.pos.x << "," << m_BehaviorGenerator.state.pos.y << "," << m_BehaviorGenerator.state.pos.z << "," << UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a)+M_PI << ","; ++ m_LogData.push_back(dataLine.str()); ++ ++ ++ if(bWayGlobalPathLogs) ++ { ++ for(unsigned int i=0; i < m_GlobalPaths.size(); i++) ++ { ++ std::ostringstream str_out; ++ str_out << UtilityHNS::UtilityH::GetHomeDirectory(); ++ str_out << UtilityHNS::DataRW::LoggingMainfolderName; ++ str_out << UtilityHNS::DataRW::PathLogFolderName; ++ str_out << "Global_Vel"; ++ str_out << i; ++ str_out << "_"; ++ PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(), m_GlobalPaths.at(i)); ++ } ++ bWayGlobalPathLogs = false; ++ } ++} ++ ++void BehaviorGen::CalculateTurnAngle(PlannerHNS::WayPoint turn_point){ ++ geometry_msgs::PoseStamped turn_pose; ++ ++ if(GetBaseMapTF()){ ++ // std::cout<<"BEFORE:"<("/op_behavior_selector/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv"); ++ private_nh.param("/op_behavior_selector/rate", rate, 10); ++ private_nh.param("/op_behavior_selector/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); ++ private_nh.param("/op_behavior_selector/task_execution_time", task_execution_time, (double)10); ++ private_nh.param("/op_behavior_selector/task_relative_deadline", task_relative_deadline, (double)10); ++ ++ /* For Task scheduling */ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ m_sprintSwitch = false; ++ ++ ros::Rate r(100); ++ while(ros::ok()){ ++ rubis::start_task_profiling(); ++ ++ ros::spinOnce(); ++ ++ // Check Pedestrian is Appeared ++ double dt = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer); ++ UtilityHNS::UtilityH::GetTickCount(planningTimer); ++ ++ if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); ++ } ++ else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); ++ ++ } ++ else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) ++ { ++ std::vector conn_data;; ++ ++ if(m_MapRaw.GetVersion()==2) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, ++ m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); ++ ++ try{ ++ // Add Traffic Signal Info from yaml file ++ XmlRpc::XmlRpcValue traffic_light_list; ++ nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list); ++ ++ // Add Stop Line Info from yaml file ++ XmlRpc::XmlRpcValue stop_line_list; ++ nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list); ++ ++ // Add Crossing Info from yaml file ++ // XmlRpc::XmlRpcValue intersection_list; ++ // nh.getParam("/op_behavior_selector/intersection_list", intersection_list); ++ ++ PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list); ++ } ++ catch(XmlRpc::XmlRpcException& e){ ++ ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str()); ++ exit(1); ++ } ++ ++ m_BehaviorGenerator.m_Map = m_Map; ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl; ++ } ++ } ++ else if(m_MapRaw.GetVersion()==1) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl; ++ } ++ } ++ } ++ ++ if(bNewCurrentPos && m_GlobalPaths.size()>0) ++ { ++ if(bNewLightSignal) ++ { ++ m_PrevTrafficLight = m_CurrTrafficLight; ++ bNewLightSignal = false; ++ } ++ ++ if(bNewLightStatus) ++ { ++ bNewLightStatus = false; ++ for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++) ++ m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus; ++ } ++ ++ m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch; ++ m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0); ++ std_msgs::Int32 curr_state_msg; ++ curr_state_msg.data = m_CurrentBehavior.state; ++ ++ pub_currentState.publish(curr_state_msg); ++ ++ CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint); ++ m_BehaviorGenerator.m_turnAngle = m_turnAngle; ++ ++ std_msgs::Float64 turn_angle_msg; ++ turn_angle_msg.data = m_turnAngle; ++ pub_turnAngle.publish(turn_angle_msg); ++ ++ emergency_stop_msg.data = false; ++ if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop! ++ emergency_stop_msg.data = true; ++ pub_EmergencyStop.publish(emergency_stop_msg); ++ ++ SendLocalPlanningTopics(lane_array_with_pose_twist_msg_); ++ VisualizeLocalPlanner(); ++ LogLocalPlanningInfo(dt); ++ ++ // Publish turn_marker ++ visualization_msgs::MarkerArray turn_marker; ++ visualization_msgs::Marker marker; ++ marker.header.frame_id = "map"; ++ marker.type = 2; ++ marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x; ++ marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y; ++ marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z; ++ marker.scale.x = 3; ++ marker.scale.y = 3; ++ marker.scale.z = 3; ++ marker.color.r = 0.0f; ++ marker.color.g = 1.0f; ++ marker.color.b = 0.0f; ++ marker.color.a = 1.0f; ++ marker.header.stamp = ros::Time::now(); ++ marker.header.frame_id = "map"; ++ turn_marker.markers.push_back(marker); ++ ++ pub_turnMarker.publish(turn_marker); ++ } ++ else ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); ++ ++ rubis::stop_task_profiling(0, 0); ++ r.sleep(); ++ } ++} ++ ++bool BehaviorGen::GetBaseMapTF(){ ++ ++ try{ ++ m_map_base_listener.waitForTransform("base_link", "map", ros::Time(0), ros::Duration(0.001)); ++ m_map_base_listener.lookupTransform("base_link", "map", ros::Time(0), m_map_base_transform); ++ return true; ++ } ++ catch(tf::TransformException& ex) ++ { ++ return false; ++ } ++ ++} ++ ++void BehaviorGen::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped& out_pose, const tf::StampedTransform &in_transform) ++{ ++ ++ tf::Vector3 in_pos(in_pose.pose.position.x, ++ in_pose.pose.position.y, ++ in_pose.pose.position.z); ++ tf::Quaternion in_quat(in_pose.pose.orientation.x, ++ in_pose.pose.orientation.y, ++ in_pose.pose.orientation.w, ++ in_pose.pose.orientation.z); ++ ++ tf::Vector3 in_pos_t = in_transform * in_pos; ++ tf::Quaternion in_quat_t = in_transform * in_quat; ++ ++ out_pose.header = in_pose.header; ++ out_pose.pose.position.x = in_pos_t.x(); ++ out_pose.pose.position.y = in_pos_t.y(); ++ out_pose.pose.position.z = in_pos_t.z(); ++ out_pose.pose.orientation.x = in_quat_t.x(); ++ out_pose.pose.orientation.y = in_quat_t.y(); ++ out_pose.pose.orientation.z = in_quat_t.z(); ++ ++ return; ++} ++ ++//Mapping Section ++ ++void BehaviorGen::callbackGetVMLanes(const vector_map_msgs::LaneArray& msg) ++{ ++ std::cout << "Received Lanes" << endl; ++ if(m_MapRaw.pLanes == nullptr) ++ m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMPoints(const vector_map_msgs::PointArray& msg) ++{ ++ std::cout << "Received Points" << endl; ++ if(m_MapRaw.pPoints == nullptr) ++ m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMdtLanes(const vector_map_msgs::DTLaneArray& msg) ++{ ++ std::cout << "Received dtLanes" << endl; ++ if(m_MapRaw.pCenterLines == nullptr) ++ m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMIntersections(const vector_map_msgs::CrossRoadArray& msg) ++{ ++ std::cout << "Received CrossRoads" << endl; ++ if(m_MapRaw.pIntersections == nullptr) ++ m_MapRaw.pIntersections = new UtilityHNS::AisanIntersectionFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMAreas(const vector_map_msgs::AreaArray& msg) ++{ ++ std::cout << "Received Areas" << endl; ++ if(m_MapRaw.pAreas == nullptr) ++ m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMLines(const vector_map_msgs::LineArray& msg) ++{ ++ std::cout << "Received Lines" << endl; ++ if(m_MapRaw.pLines == nullptr) ++ m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMStopLines(const vector_map_msgs::StopLineArray& msg) ++{ ++ std::cout << "Received StopLines" << endl; ++ if(m_MapRaw.pStopLines == nullptr) ++ m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMSignal(const vector_map_msgs::SignalArray& msg) ++{ ++ std::cout << "Received Signals" << endl; ++ if(m_MapRaw.pSignals == nullptr) ++ m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMVectors(const vector_map_msgs::VectorArray& msg) ++{ ++ std::cout << "Received Vectors" << endl; ++ if(m_MapRaw.pVectors == nullptr) ++ m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMCurbs(const vector_map_msgs::CurbArray& msg) ++{ ++ std::cout << "Received Curbs" << endl; ++ if(m_MapRaw.pCurbs == nullptr) ++ m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMRoadEdges(const vector_map_msgs::RoadEdgeArray& msg) ++{ ++ std::cout << "Received Edges" << endl; ++ if(m_MapRaw.pRoadedges == nullptr) ++ m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMWayAreas(const vector_map_msgs::WayAreaArray& msg) ++{ ++ std::cout << "Received Wayareas" << endl; ++ if(m_MapRaw.pWayAreas == nullptr) ++ m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMCrossWalks(const vector_map_msgs::CrossWalkArray& msg) ++{ ++ std::cout << "Received CrossWalks" << endl; ++ if(m_MapRaw.pCrossWalks == nullptr) ++ m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMNodes(const vector_map_msgs::NodeArray& msg) ++{ ++ std::cout << "Received Nodes" << endl; ++ if(m_MapRaw.pNodes == nullptr) ++ m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg); ++} ++} +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp +index d31d230c..b4b55585 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp +@@ -76,16 +76,16 @@ MotionPrediction::MotionPrediction() + object_msg_list_.push_back(msg); + } + +- sub_current_pose = nh.subscribe("/current_pose", 10, &MotionPrediction::callbackGetCurrentPose, this); ++ sub_current_pose = nh.subscribe("/current_pose", 1, &MotionPrediction::callbackGetCurrentPose, this); // Def: 1 + + int bVelSource = 1; + _nh.getParam("/op_motion_predictor/velocitySource", bVelSource); + if(bVelSource == 0) +- sub_robot_odom = nh.subscribe("/odom", 10, &MotionPrediction::callbackGetRobotOdom, this); ++ sub_robot_odom = nh.subscribe("/odom", 1, &MotionPrediction::callbackGetRobotOdom, this); // Def: 1 + else if(bVelSource == 1) +- sub_current_velocity = nh.subscribe("/current_velocity", 10, &MotionPrediction::callbackGetVehicleStatus, this); ++ sub_current_velocity = nh.subscribe("/current_velocity", 1, &MotionPrediction::callbackGetVehicleStatus, this); // Def: 1 + else if(bVelSource == 2) +- sub_can_info = nh.subscribe("/can_info", 10, &MotionPrediction::callbackGetCANInfo, this); ++ sub_can_info = nh.subscribe("/can_info", 1, &MotionPrediction::callbackGetCANInfo, this); // Def: 1 + + UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer); + PlannerHNS::ROSHelpers::InitPredMarkers(100, m_PredictedTrajectoriesDummy); +@@ -245,8 +245,6 @@ void MotionPrediction::callbackGetVehicleStatus(const geometry_msgs::TwistStampe + m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); + } + + void MotionPrediction::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) +@@ -405,16 +403,77 @@ void MotionPrediction::callbackGetTrackedObjects(const autoware_msgs::DetectedOb + } + } + +-void MotionPrediction::callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& in_msg) ++void MotionPrediction::callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& in_msg){ ++ rubis::start_task_profiling(); ++ ++ rubis_msgs::DetectedObjectArray msg = *in_msg; ++ _callbackGetRubisTrackedObjects(msg); ++ ++ if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); ++ } ++ else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); ++ } ++ else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) ++ { ++ std::vector conn_data;; ++ ++ if(m_MapRaw.GetVersion()==2) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, ++ m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, true); ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V2 Is Loaded successfully from the Motion Predictor !! " << std::endl; ++ } ++ } ++ else if(m_MapRaw.GetVersion()==1) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true); ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V1 Is Loaded successfully from the Motion Predictor !! " << std::endl; ++ } ++ } ++ } ++ ++ if(UtilityHNS::UtilityH::GetTimeDiffNow(m_VisualizationTimer) > m_VisualizationTime) ++ { ++ VisualizePrediction(); ++ UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer); ++ } ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++} ++ ++void MotionPrediction::_callbackGetRubisTrackedObjects(rubis_msgs::DetectedObjectArray& objects_msg) + { +- rubis::instance_ = in_msg->instance; ++ rubis::instance_ = objects_msg.instance; ++ rubis::obj_instance_ = objects_msg.instance; + + UtilityHNS::UtilityH::GetTickCount(m_SensingTimer); + m_TrackedObjects.clear(); + bTrackedObjects = true; + + // Check frame id of the object is valid +- std::string target_frame = in_msg->object_array.header.frame_id; ++ std::string target_frame = objects_msg.object_array.header.frame_id; + int obj_idx = getIndex(tf_str_list_, target_frame); + if(obj_idx == -1){ + std::cout<object_array, transform_list_[obj_idx]); ++ msg = TrasformObjAryToVeldoyne(objects_msg.object_array, transform_list_[obj_idx]); + } + else{ +- msg = in_msg->object_array; ++ msg = objects_msg.object_array; + } + + +@@ -496,9 +555,10 @@ void MotionPrediction::callbackGetRubisTrackedObjects(const rubis_msgs::Detected + + rubis_msgs::DetectedObjectArray output_msg; + output_msg.instance = rubis::instance_; ++ output_msg.obj_instance = rubis::obj_instance_; + output_msg.object_array = m_PredictedResultsResults; + pub_rubis_predicted_objects_trajectories.publish(output_msg); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + } + + } +@@ -643,99 +703,29 @@ void MotionPrediction::MainLoop() + + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- +- private_nh.param("/op_motion_predictor/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_motion_predictor/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/op_motion_predictor/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_motion_predictor.csv"); +- private_nh.param("/op_motion_predictor/rate", rate, 10); +- private_nh.param("/op_motion_predictor/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/op_motion_predictor/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/op_motion_predictor/task_relative_deadline", task_relative_deadline, (double)10); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_motion_predictor.csv"); + +- ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25); +- +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) +- { +- bMap = true; +- PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); +- } +- else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) +- { +- bMap = true; +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); +- } +- else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) +- { +- std::vector conn_data;; +- +- if(m_MapRaw.GetVersion()==2) +- { +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, +- m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, +- m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, +- m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, +- m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, +- m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, true); +- +- if(m_Map.roadSegments.size() > 0) +- { +- bMap = true; +- std::cout << " ******* Map V2 Is Loaded successfully from the Motion Predictor !! " << std::endl; +- } +- } +- else if(m_MapRaw.GetVersion()==1) +- { +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, +- m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, +- m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, +- m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, +- m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true); +- +- if(m_Map.roadSegments.size() > 0) +- { +- bMap = true; +- std::cout << " ******* Map V1 Is Loaded successfully from the Motion Predictor !! " << std::endl; +- } +- } +- } +- +- if(UtilityHNS::UtilityH::GetTimeDiffNow(m_VisualizationTimer) > m_VisualizationTime) +- { +- VisualizePrediction(); +- UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer); +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } ++ rubis::init_task_profiling(task_response_time_filename); + +- loop_rate.sleep(); +- } ++ ros::spin(); + } + + void MotionPrediction::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped& out_pose, const tf::StampedTransform &in_transform) +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp +index d73353ad..5ff9be73 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp +@@ -30,6 +30,7 @@ TrajectoryEval::TrajectoryEval() + bWayGlobalPathToUse = false; + m_bUseMoveingObjectsPrediction = false; + m_noVehicleCnt = 0; ++ is_objects_updated_ = false; + + ros::NodeHandle _nh; + UpdatePlanningParams(_nh); +@@ -42,42 +43,29 @@ TrajectoryEval::TrajectoryEval() + + pub_CollisionPointsRviz = nh.advertise("dynamic_collision_points_rviz", 1); + pub_LocalWeightedTrajectoriesRviz = nh.advertise("local_trajectories_eval_rviz", 1); +- pub_LocalWeightedTrajectories = nh.advertise("local_weighted_trajectories", 1); ++ // pub_LocalWeightedTrajectories = nh.advertise("local_weighted_trajectories", 1); ++ pub_LocalWeightedTrajectoriesWithPoseTwist = nh.advertise("local_weighted_trajectories_with_pose_twist", 1); + pub_TrajectoryCost = nh.advertise("local_trajectory_cost", 1); + pub_SafetyBorderRviz = nh.advertise("safety_border", 1); + pub_DistanceToPedestrian = nh.advertise("distance_to_pedestrian", 1); + pub_IntersectionCondition = nh.advertise("intersection_condition", 1); + pub_SprintSwitch = nh.advertise("sprint_switch", 1); + +- sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this); +- sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this); +- +- int bVelSource = 1; +- _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); +- if(bVelSource == 0) +- sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this); +- else if(bVelSource == 1) +- sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this); +- else if(bVelSource == 2) +- sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this); +- +- /* RT Scheduling setup */ +- // sub_current_pose = nh.subscribe("/current_pose", 1, &TrajectoryEval::callbackGetCurrentPose, this); //origin 10 +- // sub_current_state = nh.subscribe("/current_state", 1, &TrajectoryEval::callbackGetCurrentState, this); //origin 10 ++ // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this); ++ // sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this); + + // int bVelSource = 1; + // _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); + // if(bVelSource == 0) +- // sub_robot_odom = nh.subscribe("/odom", 1, &TrajectoryEval::callbackGetRobotOdom, this); //origin 10 ++ // sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this); + // else if(bVelSource == 1) +- // sub_current_velocity = nh.subscribe("/current_velocity", 1, &TrajectoryEval::callbackGetVehicleStatus, this); //origin 10 ++ // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this); + // else if(bVelSource == 2) +- // sub_can_info = nh.subscribe("/can_info", 1, &TrajectoryEval::callbackGetCANInfo, this); //origin 10 ++ // sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this); + + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); +- sub_LocalPlannerPaths = nh.subscribe("/local_trajectories", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this); +- // sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); +- sub_rubis_predicted_objects = nh.subscribe("/rubis_predicted_objects", 1, &TrajectoryEval::callbackGetRubisPredictedObjects, this); ++ sub_LocalPlannerPaths = nh.subscribe("/local_trajectories_with_pose_twist", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this); ++ sub_predicted_objects = nh.subscribe("/rubis_predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); + sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this); + + PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy); +@@ -155,23 +143,21 @@ void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh) + + } + +-void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +-{ +- m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); +- bNewCurrentPos = true; +-} +- +-void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) +-{ +- m_VehicleStatus.speed = msg->twist.linear.x; +- m_CurrentPos.v = m_VehicleStatus.speed; +- if(fabs(msg->twist.linear.x) > 0.25) +- m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); +- UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); +- bVehicleStatus = true; +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +-} ++// void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) ++// { ++// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// bNewCurrentPos = true; ++// } ++ ++// void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) ++// { ++// m_VehicleStatus.speed = msg->twist.linear.x; ++// m_CurrentPos.v = m_VehicleStatus.speed; ++// if(fabs(msg->twist.linear.x) > 0.25) ++// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++// bVehicleStatus = true; ++// } + + void TrajectoryEval::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) + { +@@ -226,17 +212,51 @@ void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArray + } + } + +-void TrajectoryEval::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++void TrajectoryEval::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) + { +- if(msg->lanes.size() > 0) ++ rubis::start_task_profiling(); ++ // Before spin ++ UpdateMyParams(); ++ UpdateTf(); ++ ++ // callback for objects ++ if(is_objects_updated_){ ++ _callbackGetPredictedObjects(object_msg_); ++ is_objects_updated_ = false; ++ } ++ ++ static double prev_x = 0.0, prev_y = 0.0, prev_speed = 0.0; ++ ++ // callback for current pose ++ if(prev_x != msg->pose.pose.position.x || prev_y != msg->pose.pose.position.y){ ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ bNewCurrentPos = true; ++ prev_x = msg->pose.pose.position.x; ++ prev_y = msg->pose.pose.position.y; ++ } ++ ++ // callback for vehicle status ++ if(prev_speed != msg->twist.twist.linear.x){ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ prev_speed = msg->twist.twist.linear.x; ++ } ++ ++ ++ // callback for local planner path ++ if(msg->lane_array.lanes.size() > 0) + { + m_GeneratedRollOuts.clear(); + int globalPathId_roll_outs = -1; + +- for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) + { + std::vector path; +- PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), path); ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); + m_GeneratedRollOuts.push_back(path); + if(path.size() > 0) + globalPathId_roll_outs = path.at(0).gid; +@@ -257,157 +277,140 @@ void TrajectoryEval::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayC + + bRollOuts = true; + } +-} +- +-void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg) +-{ +- /* +- m_PredictedObjects.clear(); +- bPredictedObjects = true; +- double distance_to_pedestrian = 1000; +- int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2; +- int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2; +- +- int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2; +- int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2; + +- int vehicle_cnt = 0; ++ // After spin ++ PlannerHNS::TrajectoryCost tc; + +- PlannerHNS::DetectedObject obj; +- for(unsigned int i = 0 ; i objects.size(); i++) +- { +- if(msg->objects.at(i).pose.position.y < -20 || msg->objects.at(i).pose.position.y > 20) +- continue; +- +- if(msg->objects.at(i).pose.position.z > 1 || msg->objects.at(i).pose.position.z < -1.5) +- continue; ++ if(bNewCurrentPos && m_GlobalPaths.size()>0) ++ { ++ m_GlobalPathSections.clear(); + +- autoware_msgs::DetectedObject msg_obj = msg->objects.at(i); ++ for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++) ++ { ++ t_centerTrajectorySmoothed.clear(); ++ PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); ++ m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); ++ } + +- // #### Decison making for objects +- +- if(msg_obj.id > 0) // If fusion object is detected ++ if(m_GlobalPathSections.size()>0) + { +- if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ +- vehicle_cnt += 1; +- } ++ if(m_bUseMoveingObjectsPrediction) ++ tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory); ++ else ++ tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state); ++ ++ autoware_msgs::Lane l; ++ l.closest_object_distance = tc.closest_obj_distance; ++ l.closest_object_velocity = tc.closest_obj_velocity; ++ l.cost = tc.cost; ++ l.is_blocked = tc.bBlocked; ++ l.lane_index = tc.index; ++ pub_TrajectoryCost.publish(l); ++ ++ // hjw added : Check if ego is on intersection and obstacles are in risky area ++ int intersectionID = -1; ++ double closestIntersectionDistance = -1; ++ bool isInsideIntersection = false; ++ bool riskyLeftTurn = false; ++ bool riskyRightTurn = false; ++ ++ PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list_, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn); ++ ++ autoware_msgs::IntersectionCondition ic_msg; ++ ic_msg.intersectionID = intersectionID; ++ ic_msg.intersectionDistance = closestIntersectionDistance; ++ ic_msg.isIntersection = isInsideIntersection; ++ ic_msg.riskyLeftTurn = riskyLeftTurn; ++ ic_msg.riskyRightTurn = riskyRightTurn; ++ ++ pub_IntersectionCondition.publish(ic_msg); + +- PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj); ++ } + +- // transform center pose into map frame +- geometry_msgs::PoseStamped pose_in_map; +- pose_in_map.header = msg_obj.header; +- pose_in_map.pose = msg_obj.pose; +- try{ +- m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map); +- } +- catch(tf::TransformException& ex) ++ if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) ++ { ++ rubis_msgs::LaneArrayWithPoseTwist local_lanes; ++ for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++) + { +- // ROS_ERROR("Cannot transform object pose: %s", ex.what()); +- continue; ++ autoware_msgs::Lane lane; ++ PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane); ++ lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance; ++ lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity; ++ lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost; ++ lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked; ++ lane.lane_index = i; ++ local_lanes.lane_array.lanes.push_back(lane); + } +- // msg_obj.header.frame_id = "map"; +- obj.center.pos.x = pose_in_map.pose.position.x; +- obj.center.pos.y = pose_in_map.pose.position.y; +- obj.center.pos.z = pose_in_map.pose.position.z; +- +- // transform contour into map frame +- for(unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++){ +- geometry_msgs::PoseStamped contour_point_in_map; +- contour_point_in_map.header = msg_obj.header; +- contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x; +- contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y; +- contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z; +- +- // For resolve TF malform, set orientation w to 1 +- contour_point_in_map.pose.orientation.w = 1; + +- try{ +- m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map); +- } +- catch(tf::TransformException& ex){ +- // ROS_ERROR("Cannot transform contour pose: %s", ex.what()); +- continue; +- } ++ rubis::instance_ = msg->instance; ++ local_lanes.instance = rubis::instance_; ++ local_lanes.obj_instance = rubis::obj_instance_; ++ local_lanes.pose = msg->pose; ++ local_lanes.twist = msg->twist; + +- obj.contour.at(j).x = contour_point_in_map.pose.position.x; +- obj.contour.at(j).y = contour_point_in_map.pose.position.y; +- obj.contour.at(j).z = contour_point_in_map.pose.position.z; +- } ++ pub_LocalWeightedTrajectoriesWithPoseTwist.publish(local_lanes); ++ // pub_LocalWeightedTrajectories.publish(local_lanes.lane_array); ++ } ++ else ++ { ++ ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()"); ++ } + +- msg_obj.header.frame_id = "map"; ++ if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0) ++ { ++ visualization_msgs::MarkerArray all_rollOuts; ++ PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts); ++ pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts); + +- m_PredictedObjects.push_back(obj); +- } ++ PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy); ++ pub_CollisionPointsRviz.publish(m_CollisionsActual); + +- int image_obj_center_x = msg_obj.x+msg_obj.width/2; +- int image_obj_center_y = msg_obj.y+msg_obj.height/2; +- if (msg_obj.label == "person"){// If person is detected only in image +- +- // TO ERASE +- // ROS_WARN("object height:%d // thr: %d\n", msg_obj.height, m_pedestrian_stop_img_height_threshold); +- printf("center_x %d \n left: %d \n right %d\n\n\n", image_obj_center_x, image_person_detection_range_left, image_person_detection_range_right); +- if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ +- double temp_x_distance = 1000; +- if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; +- if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; +- } ++ //Visualize Safety Box ++ visualization_msgs::Marker safety_box; ++ PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box); ++ pub_SafetyBorderRviz.publish(safety_box); + } + } ++ else ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); + +- // Publish Sprint Switch +- std_msgs::Bool sprint_switch_msg; + +- if(vehicle_cnt != 0){ +- m_noVehicleCnt = 0; +- sprint_switch_msg.data = false; +- } +- else{ // No vehicle is exist in front of the car +- if(m_noVehicleCnt < m_SprintDecisionTime*10) { +- m_noVehicleCnt +=1; +- sprint_switch_msg.data = false; +- } +- else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true; +- } +- pub_SprintSwitch.publish(sprint_switch_msg); ++ rubis::stop_task_profiling(rubis::instance_, 0); ++} + +- std_msgs::Float64 distanceToPedestrianMsg; +- distanceToPedestrianMsg.data = distance_to_pedestrian; +- pub_DistanceToPedestrian.publish(distanceToPedestrianMsg); +- */ ++void TrajectoryEval::callbackGetPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg) ++{ ++ object_msg_ = msg->object_array; ++ rubis::obj_instance_ = msg->obj_instance; ++ is_objects_updated_ = true; + } + +-void TrajectoryEval::callbackGetRubisPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg) +-{ ++void TrajectoryEval::_callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArray& objects_msg){ + m_PredictedObjects.clear(); + bPredictedObjects = true; + double distance_to_pedestrian = 1000; + int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2; + int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2; +- + int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2; + int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2; +- + int vehicle_cnt = 0; + + PlannerHNS::DetectedObject obj; +- +- for(unsigned int i = 0 ; i object_array.objects.size(); i++) ++ for(unsigned int i = 0 ; i object_array.objects.at(i).pose.position.y < -20 || msg->object_array.objects.at(i).pose.position.y > 20) ++ if(objects_msg.objects.at(i).pose.position.y < -20 || objects_msg.objects.at(i).pose.position.y > 20) + continue; + +- if(msg->object_array.objects.at(i).pose.position.z > 1 || msg->object_array.objects.at(i).pose.position.z < -1.5) ++ if(objects_msg.objects.at(i).pose.position.z > 1 || objects_msg.objects.at(i).pose.position.z < -1.5) + continue; + +- autoware_msgs::DetectedObject msg_obj = msg->object_array.objects.at(i); ++ autoware_msgs::DetectedObject msg_obj = objects_msg.objects.at(i); + + if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ + vehicle_cnt += 1; + } + +- PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->object_array.objects.at(i), obj); +- ++ PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(objects_msg.objects.at(i), obj); + geometry_msgs::PoseStamped pose_in_map; + pose_in_map.header = msg_obj.header; + pose_in_map.pose = msg_obj.pose; +@@ -468,7 +471,6 @@ void TrajectoryEval::callbackGetRubisPredictedObjects(const rubis_msgs::Detected + if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; + } + } +- + } + + // Publish Sprint Switch +@@ -485,6 +487,7 @@ void TrajectoryEval::callbackGetRubisPredictedObjects(const rubis_msgs::Detected + } + else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true; + } ++ + pub_SprintSwitch.publish(sprint_switch_msg); + + std_msgs::Float64 distanceToPedestrianMsg; +@@ -510,6 +513,11 @@ void TrajectoryEval::UpdateMyParams() + _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong); + _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat); + _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance); ++ ++ _nh.getParam("/op_trajectory_evaluator/lateralBlockingThreshold", m_PlanningParams.lateralBlockingThreshold); ++ _nh.getParam("/op_trajectory_evaluator/frontLongitudinalBlockingThreshold", m_PlanningParams.frontLongitudinalBlockingThreshold); ++ _nh.getParam("/op_trajectory_evaluator/rearLongitudinalBlockingThreshold", m_PlanningParams.rearLongitudinalBlockingThreshold); ++ _nh.getParam("/op_trajectory_evaluator/enableDebug", m_PlanningParams.enableDebug); + } + + bool TrajectoryEval::UpdateTf() +@@ -533,150 +541,35 @@ void TrajectoryEval::MainLoop() + { + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"); ++ + int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; ++ private_nh.param(node_name+"/rate", rate, 10); + +- private_nh.param("/op_trajectory_evaluator/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_trajectory_evaluator/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/op_trajectory_evaluator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"); +- private_nh.param("/op_trajectory_evaluator/rate", rate, 10); +- private_nh.param("/op_trajectory_evaluator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/op_trajectory_evaluator/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/op_trajectory_evaluator/task_relative_deadline", task_relative_deadline, (double)10); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + PlannerHNS::WayPoint prevState, state_change; + + // Add Crossing Info from yaml file +- XmlRpc::XmlRpcValue intersection_xml; +- std::vector intersection_list; ++ XmlRpc::XmlRpcValue intersection_xml; + nh.getParam("/op_trajectory_evaluator/intersection_list", intersection_xml); +- PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list, intersection_xml); +- +- +- +- ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(100); ++ PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list_, intersection_xml); + +- struct timespec start_time, end_time; +- +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- UpdateMyParams(); +- UpdateTf(); +- +- ros::spinOnce(); +- PlannerHNS::TrajectoryCost tc; +- +- if(bNewCurrentPos && m_GlobalPaths.size()>0) +- { +- m_GlobalPathSections.clear(); +- +- for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++) +- { +- t_centerTrajectorySmoothed.clear(); +- PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); +- m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); +- } +- +- if(m_GlobalPathSections.size()>0) +- { +- if(m_bUseMoveingObjectsPrediction) +- tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory); +- else +- tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state); +- +- autoware_msgs::Lane l; +- l.closest_object_distance = tc.closest_obj_distance; +- l.closest_object_velocity = tc.closest_obj_velocity; +- l.cost = tc.cost; +- l.is_blocked = tc.bBlocked; +- l.lane_index = tc.index; +- pub_TrajectoryCost.publish(l); +- +- // hjw added : Check if ego is on intersection and obstacles are in risky area +- int intersectionID = -1; +- double closestIntersectionDistance = -1; +- bool isInsideIntersection = false; +- bool riskyLeftTurn = false; +- bool riskyRightTurn = false; +- +- PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn); +- +- autoware_msgs::IntersectionCondition ic_msg; +- ic_msg.intersectionID = intersectionID; +- ic_msg.intersectionDistance = closestIntersectionDistance; +- ic_msg.isIntersection = isInsideIntersection; +- ic_msg.riskyLeftTurn = riskyLeftTurn; +- ic_msg.riskyRightTurn = riskyRightTurn; +- +- pub_IntersectionCondition.publish(ic_msg); +- +- } +- +- if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) +- { +- autoware_msgs::LaneArray local_lanes; +- for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++) +- { +- autoware_msgs::Lane lane; +- PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane); +- lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance; +- lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity; +- lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost; +- lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked; +- lane.lane_index = i; +- local_lanes.lanes.push_back(lane); +- } +- +- pub_LocalWeightedTrajectories.publish(local_lanes); +- rubis::sched::task_state_ = TASK_STATE_DONE; +- } +- else +- { +- ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()"); +- } +- +- if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0) +- { +- visualization_msgs::MarkerArray all_rollOuts; +- PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts); +- pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts); +- +- PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy); +- pub_CollisionPointsRviz.publish(m_CollisionsActual); +- +- //Visualize Safety Box +- visualization_msgs::Marker safety_box; +- PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box); +- pub_SafetyBorderRviz.publish(safety_box); +- } +- } +- else +- sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- loop_rate.sleep(); +- } ++ ros::spin(); + } +- + } +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp +new file mode 100644 +index 00000000..a1ce5e83 +--- /dev/null ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp +@@ -0,0 +1,596 @@ ++/* ++ * Copyright 2018-2019 Autoware Foundation. All rights reserved. ++ * ++ * Licensed under the Apache License, Version 2.0 (the "License"); ++ * you may not use this file except in compliance with the License. ++ * You may obtain a copy of the License at ++ * ++ * http://www.apache.org/licenses/LICENSE-2.0 ++ * ++ * Unless required by applicable law or agreed to in writing, software ++ * distributed under the License is distributed on an "AS IS" BASIS, ++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ++ * See the License for the specific language governing permissions and ++ * limitations under the License. ++ */ ++ ++#include "op_trajectory_evaluator_core.h" ++#include "op_ros_helpers/op_ROSHelpers.h" ++#include "op_planner/MappingHelpers.h" ++#include ++ ++namespace TrajectoryEvaluatorNS ++{ ++ ++TrajectoryEval::TrajectoryEval() ++{ ++ bNewCurrentPos = false; ++ bVehicleStatus = false; ++ bWayGlobalPath = false; ++ bWayGlobalPathToUse = false; ++ m_bUseMoveingObjectsPrediction = false; ++ m_noVehicleCnt = 0; ++ ++ ros::NodeHandle _nh; ++ UpdatePlanningParams(_nh); ++ ++ tf::StampedTransform transform; ++ PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); ++ m_OriginPos.position.x = transform.getOrigin().x(); ++ m_OriginPos.position.y = transform.getOrigin().y(); ++ m_OriginPos.position.z = transform.getOrigin().z(); ++ ++ pub_CollisionPointsRviz = nh.advertise("dynamic_collision_points_rviz", 1); ++ pub_LocalWeightedTrajectoriesRviz = nh.advertise("local_trajectories_eval_rviz", 1); ++ pub_LocalWeightedTrajectories = nh.advertise("local_weighted_trajectories", 1); ++ pub_LocalWeightedTrajectoriesWithPoseTwist = nh.advertise("local_weighted_trajectories_with_pose_twist", 1); ++ pub_TrajectoryCost = nh.advertise("local_trajectory_cost", 1); ++ pub_SafetyBorderRviz = nh.advertise("safety_border", 1); ++ pub_DistanceToPedestrian = nh.advertise("distance_to_pedestrian", 1); ++ pub_IntersectionCondition = nh.advertise("intersection_condition", 1); ++ pub_SprintSwitch = nh.advertise("sprint_switch", 1); ++ ++ // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this); ++ // sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this); ++ ++ int bVelSource = 1; ++ _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); ++ if(bVelSource == 0) ++ sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this); ++ // else if(bVelSource == 1) ++ // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this); ++ else if(bVelSource == 2) ++ sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this); ++ ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); ++ sub_LocalPlannerPaths = nh.subscribe("/local_trajectories_with_pose_twist", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this); ++ sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); ++ sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this); ++ ++ PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy); ++ ++ while(1){ ++ if(UpdateTf() == true) ++ break; ++ } ++} ++ ++TrajectoryEval::~TrajectoryEval() ++{ ++} ++ ++void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh) ++{ ++ _nh.getParam("/op_trajectory_evaluator/enablePrediction", m_bUseMoveingObjectsPrediction); ++ ++ _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); ++ _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); ++ _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); ++ if(m_PlanningParams.enableSwerving) ++ m_PlanningParams.enableFollowing = true; ++ else ++ _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); ++ ++ _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); ++ _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); ++ ++ _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); ++ _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); ++ _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); ++ ++ _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); ++ ++ _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); ++ if(m_PlanningParams.enableSwerving) ++ _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); ++ else ++ m_PlanningParams.rollOutNumber = 0; ++ ++ std::cout << "Rolls Number: " << m_PlanningParams.rollOutNumber << std::endl; ++ ++ _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); ++ _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); ++ _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); ++ _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); ++ _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); ++ ++ _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); ++ ++ _nh.getParam("/op_common_params/width", m_CarInfo.width); ++ _nh.getParam("/op_common_params/length", m_CarInfo.length); ++ _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); ++ _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); ++ _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); ++ _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); ++ _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); ++ m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; ++ m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; ++ ++ _nh.param("/op_trajectory_evaluator/PedestrianRightThreshold", m_PedestrianRightThreshold, 7.0); ++ _nh.param("/op_trajectory_evaluator/PedestrianLeftThreshold", m_PedestrianLeftThreshold, 2.0); ++ _nh.param("/op_trajectory_evaluator/PedestrianImageDetectionRange", m_PedestrianImageDetectionRange, 0.7); ++ _nh.param("/op_trajectory_evaluator/PedestrianStopImgHeightThreshold", m_pedestrian_stop_img_height_threshold, 120); ++ _nh.param("/op_trajectory_evaluator/ImageWidth", m_ImageWidth, 1920); ++ _nh.param("/op_trajectory_evaluator/ImageHeight", m_ImageHeight, 1080); ++ _nh.param("/op_trajectory_evaluator/VehicleImageDetectionRange", m_VehicleImageDetectionRange, 0.3); ++ _nh.param("/op_trajectory_evaluator/VehicleImageWidthThreshold", m_VehicleImageWidthThreshold, 0.05); ++ _nh.param("/op_trajectory_evaluator/SprintDecisionTime", m_SprintDecisionTime, 5.0); ++ ++ ++ m_VehicleImageWidthThreshold = m_VehicleImageWidthThreshold * m_ImageWidth; ++ m_PedestrianRightThreshold *= -1; ++ ++} ++ ++// void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) ++// { ++// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// bNewCurrentPos = true; ++// } ++ ++// void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) ++// { ++// m_VehicleStatus.speed = msg->twist.linear.x; ++// m_CurrentPos.v = m_VehicleStatus.speed; ++// if(fabs(msg->twist.linear.x) > 0.25) ++// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++// bVehicleStatus = true; ++// } ++ ++void TrajectoryEval::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) ++{ ++ m_VehicleStatus.speed = msg->speed/3.6; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void TrajectoryEval::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) ++{ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++{ ++ if(msg->lanes.size() > 0) ++ { ++ ++ bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); ++ ++ m_GlobalPaths.clear(); ++ ++ for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ { ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); ++ ++ PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); ++ m_GlobalPaths.push_back(m_temp_path); ++ ++ if(bOldGlobalPath) ++ { ++ bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); ++ } ++ } ++ ++ if(!bOldGlobalPath) ++ { ++ bWayGlobalPath = true; ++ std::cout << "Received New Global Path Evaluator! " << std::endl; ++ } ++ else ++ { ++ m_GlobalPaths.clear(); ++ } ++ } ++} ++ ++void TrajectoryEval::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) ++{ ++ rubis::instance_ = msg->instance; ++ // Callback ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ bNewCurrentPos = true; ++ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ ++ if(msg->lane_array.lanes.size() > 0) ++ { ++ m_GeneratedRollOuts.clear(); ++ int globalPathId_roll_outs = -1; ++ ++ for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) ++ { ++ std::vector path; ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); ++ m_GeneratedRollOuts.push_back(path); ++ if(path.size() > 0) ++ globalPathId_roll_outs = path.at(0).gid; ++ } ++ ++ if(bWayGlobalPath && m_GlobalPaths.size() > 0 && m_GlobalPaths.at(0).size() > 0) ++ { ++ int globalPathId = m_GlobalPaths.at(0).at(0).gid; ++ std::cout << "Before Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; ++ ++ if(globalPathId_roll_outs == globalPathId) ++ { ++ bWayGlobalPath = false; ++ m_GlobalPathsToUse = m_GlobalPaths; ++ std::cout << "Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; ++ } ++ } ++ ++ bRollOuts = true; ++ } ++} ++ ++void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg) ++{ ++ m_PredictedObjects.clear(); ++ bPredictedObjects = true; ++ double distance_to_pedestrian = 1000; ++ int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2; ++ int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2; ++ ++ int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2; ++ int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2; ++ ++ int vehicle_cnt = 0; ++ ++ PlannerHNS::DetectedObject obj; ++ for(unsigned int i = 0 ; i objects.size(); i++) ++ { ++ if(msg->objects.at(i).pose.position.y < -20 || msg->objects.at(i).pose.position.y > 20) ++ continue; ++ ++ if(msg->objects.at(i).pose.position.z > 1 || msg->objects.at(i).pose.position.z < -1.5) ++ continue; ++ ++ autoware_msgs::DetectedObject msg_obj = msg->objects.at(i); ++ ++ // #### Decison making for objects ++ ++ if(msg_obj.id > 0) // If fusion object is detected ++ { ++ // calculate distance to person first ++ // if(msg_obj.label == "person"){ ++ // std::cout<<"Pedestrian box size(width x height):"< m_PedestrianLeftThreshold || temp_y_distance < m_PedestrianRightThreshold ) continue; ++ // if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; ++ // } ++ // catch(tf::TransformException& ex){ ++ // // ROS_ERROR("Cannot transform person pose: %s", ex.what()); ++ ++ // } ++ // } ++ ++ if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ ++ vehicle_cnt += 1; ++ } ++ ++ PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj); ++ ++ ++ // transform center pose into map frame ++ geometry_msgs::PoseStamped pose_in_map; ++ pose_in_map.header = msg_obj.header; ++ pose_in_map.pose = msg_obj.pose; ++ try{ ++ m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map); ++ } ++ catch(tf::TransformException& ex) ++ { ++ // ROS_ERROR("Cannot transform object pose: %s", ex.what()); ++ continue; ++ } ++ // msg_obj.header.frame_id = "map"; ++ obj.center.pos.x = pose_in_map.pose.position.x; ++ obj.center.pos.y = pose_in_map.pose.position.y; ++ obj.center.pos.z = pose_in_map.pose.position.z; ++ ++ // transform contour into map frame ++ for(unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++){ ++ geometry_msgs::PoseStamped contour_point_in_map; ++ contour_point_in_map.header = msg_obj.header; ++ contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x; ++ contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y; ++ contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z; ++ ++ // For resolve TF malform, set orientation w to 1 ++ contour_point_in_map.pose.orientation.w = 1; ++ ++ try{ ++ m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map); ++ } ++ catch(tf::TransformException& ex){ ++ // ROS_ERROR("Cannot transform contour pose: %s", ex.what()); ++ continue; ++ } ++ ++ obj.contour.at(j).x = contour_point_in_map.pose.position.x; ++ obj.contour.at(j).y = contour_point_in_map.pose.position.y; ++ obj.contour.at(j).z = contour_point_in_map.pose.position.z; ++ } ++ ++ msg_obj.header.frame_id = "map"; ++ ++ m_PredictedObjects.push_back(obj); ++ } ++ /* ++ else{ // If object is only detected at vision ++ int image_obj_center_x = msg_obj.x+msg_obj.width/2; ++ int image_obj_center_y = msg_obj.y+msg_obj.height/2; ++ // if (msg_obj.label == "person"){// If person is detected only in image ++ // // TO ERASE ++ // std::cout<<"object height:" << msg_obj.height << " / threshold:" << m_pedestrian_stop_img_height_threshold << std::endl; ++ // if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ ++ // double temp_x_distance = 1000; ++ // if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; ++ // if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; ++ // } ++ // } ++ // else ++ if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ ++ if((msg_obj.width > m_VehicleImageWidthThreshold) ++ && (image_obj_center_x > image_vehicle_detection_range_left) ++ && (image_obj_center_x < image_vehicle_detection_range_right) ++ ) ++ { ++ vehicle_cnt+=1; ++ } ++ } ++ } ++ */ ++ ++ int image_obj_center_x = msg_obj.x+msg_obj.width/2; ++ int image_obj_center_y = msg_obj.y+msg_obj.height/2; ++ if (msg_obj.label == "person"){// If person is detected only in image ++ ++ // TO ERASE ++ // ROS_WARN("object height:%d // thr: %d\n", msg_obj.height, m_pedestrian_stop_img_height_threshold); ++ printf("center_x %d \n left: %d \n right %d\n\n\n", image_obj_center_x, image_person_detection_range_left, image_person_detection_range_right); ++ if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ ++ double temp_x_distance = 1000; ++ if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; ++ if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; ++ } ++ } ++ ++ } ++ ++ // Publish Sprint Switch ++ std_msgs::Bool sprint_switch_msg; ++ ++ if(vehicle_cnt != 0){ ++ m_noVehicleCnt = 0; ++ sprint_switch_msg.data = false; ++ } ++ else{ // No vehicle is exist in front of the car ++ if(m_noVehicleCnt < m_SprintDecisionTime*10) { ++ m_noVehicleCnt +=1; ++ sprint_switch_msg.data = false; ++ } ++ else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true; ++ } ++ pub_SprintSwitch.publish(sprint_switch_msg); ++ ++ // ROS_INFO("object # : %d", m_PredictedObjects.size()); ++ ++ std_msgs::Float64 distanceToPedestrianMsg; ++ distanceToPedestrianMsg.data = distance_to_pedestrian; ++ pub_DistanceToPedestrian.publish(distanceToPedestrianMsg); ++} ++ ++void TrajectoryEval::callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr& msg) ++{ ++ m_CurrentBehavior.iTrajectory = msg->twist.angular.z; ++} ++ ++void TrajectoryEval::callbackGetCurrentState(const std_msgs::Int32 & msg) ++{ ++ m_CurrentBehavior.state = static_cast(msg.data); ++} ++ ++void TrajectoryEval::UpdateMyParams() ++{ ++ ros::NodeHandle _nh; ++ _nh.getParam("/op_trajectory_evaluator/weightPriority", m_PlanningParams.weightPriority); ++ _nh.getParam("/op_trajectory_evaluator/weightTransition", m_PlanningParams.weightTransition); ++ _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong); ++ _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat); ++ _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance); ++} ++ ++bool TrajectoryEval::UpdateTf() ++{ ++ try{ ++ m_vtob_listener.waitForTransform("/velodyne", "/base_link", ros::Time(0), ros::Duration(0.001)); ++ m_vtob_listener.lookupTransform("/velodyne", "/base_link", ros::Time(0), m_velodyne_to_base_link); ++ ++ m_vtom_listener.waitForTransform("/velodyne", "/map", ros::Time(0), ros::Duration(0.001)); ++ m_vtom_listener.lookupTransform("/velodyne", "/map", ros::Time(0), m_velodyne_to_map); ++ return true; ++ } ++ catch(tf::TransformException& ex){ ++ if(TF_DEBUG) ++ ROS_ERROR("%s", ex.what()); ++ return false; ++ } ++} ++ ++void TrajectoryEval::MainLoop() ++{ ++ ros::NodeHandle private_nh("~"); ++ ++ // Scheduling Setup ++ std::string task_response_time_filename; ++ int rate; ++ double task_minimum_inter_release_time; ++ double task_execution_time; ++ double task_relative_deadline; ++ ++ private_nh.param("/op_trajectory_evaluator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"); ++ private_nh.param("/op_trajectory_evaluator/rate", rate, 10); ++ private_nh.param("/op_trajectory_evaluator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); ++ private_nh.param("/op_trajectory_evaluator/task_execution_time", task_execution_time, (double)10); ++ private_nh.param("/op_trajectory_evaluator/task_relative_deadline", task_relative_deadline, (double)10); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ PlannerHNS::WayPoint prevState, state_change; ++ ++ // Add Crossing Info from yaml file ++ XmlRpc::XmlRpcValue intersection_xml; ++ nh.getParam("/op_trajectory_evaluator/intersection_list", intersection_xml); ++ PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list_, intersection_xml); ++ ++ ros::Rate r(100); ++ ++ while(ros::ok()){ ++ // Before spin ++ UpdateMyParams(); ++ UpdateTf(); ++ ++ ros::spinOnce(); ++ ++ PlannerHNS::TrajectoryCost tc; ++ ++ if(bNewCurrentPos && m_GlobalPaths.size()>0) ++ { ++ m_GlobalPathSections.clear(); ++ ++ for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++) ++ { ++ t_centerTrajectorySmoothed.clear(); ++ PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); ++ m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); ++ } ++ ++ if(m_GlobalPathSections.size()>0) ++ { ++ if(m_bUseMoveingObjectsPrediction) ++ tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory); ++ else ++ tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state); ++ ++ autoware_msgs::Lane l; ++ l.closest_object_distance = tc.closest_obj_distance; ++ l.closest_object_velocity = tc.closest_obj_velocity; ++ l.cost = tc.cost; ++ l.is_blocked = tc.bBlocked; ++ l.lane_index = tc.index; ++ pub_TrajectoryCost.publish(l); ++ ++ // hjw added : Check if ego is on intersection and obstacles are in risky area ++ int intersectionID = -1; ++ double closestIntersectionDistance = -1; ++ bool isInsideIntersection = false; ++ bool riskyLeftTurn = false; ++ bool riskyRightTurn = false; ++ ++ PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list_, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn); ++ ++ autoware_msgs::IntersectionCondition ic_msg; ++ ic_msg.intersectionID = intersectionID; ++ ic_msg.intersectionDistance = closestIntersectionDistance; ++ ic_msg.isIntersection = isInsideIntersection; ++ ic_msg.riskyLeftTurn = riskyLeftTurn; ++ ic_msg.riskyRightTurn = riskyRightTurn; ++ ++ pub_IntersectionCondition.publish(ic_msg); ++ ++ } ++ ++ if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) ++ { ++ rubis_msgs::LaneArrayWithPoseTwist local_lanes; ++ for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++) ++ { ++ autoware_msgs::Lane lane; ++ PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane); ++ lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance; ++ lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity; ++ lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost; ++ lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked; ++ lane.lane_index = i; ++ local_lanes.lane_array.lanes.push_back(lane); ++ } ++ ++ local_lanes.instance = rubis::instance_; ++ local_lanes.pose = msg->pose; ++ local_lanes.twist = msg->twist; ++ ++ pub_LocalWeightedTrajectoriesWithPoseTwist.publish(local_lanes); ++ pub_LocalWeightedTrajectories.publish(local_lanes.lane_array); ++ ++ } ++ else ++ { ++ ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()"); ++ } ++ ++ if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0) ++ { ++ visualization_msgs::MarkerArray all_rollOuts; ++ PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts); ++ pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts); ++ ++ PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy); ++ pub_CollisionPointsRviz.publish(m_CollisionsActual); ++ ++ //Visualize Safety Box ++ visualization_msgs::Marker safety_box; ++ PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box); ++ pub_SafetyBorderRviz.publish(safety_box); ++ } ++ } ++ else ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); ++ ++ rubis::stop_task_profiling(0, 0); ++ ++ r.sleep(); ++ } ++} ++ ++} +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp +index 10549a37..be9bebe5 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp +@@ -39,37 +39,24 @@ TrajectoryGen::TrajectoryGen() + m_OriginPos.position.y = transform.getOrigin().y(); + m_OriginPos.position.z = transform.getOrigin().z(); + +- pub_LocalTrajectories = nh.advertise("local_trajectories", 1); ++ // pub_LocalTrajectories = nh.advertise("local_trajectories", 1); ++ pub_LocalTrajectoriesWithPoseTwist = nh.advertise("local_trajectories_with_pose_twist", 1); + pub_LocalTrajectoriesRviz = nh.advertise("local_trajectories_gen_rviz", 1); + + sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this); +- sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryGen::callbackGetCurrentPose, this); + + int bVelSource = 1; + _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource); +- if(bVelSource == 0) +- sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryGen::callbackGetRobotOdom, this); +- else if(bVelSource == 1) +- sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this); +- else if(bVelSource == 2) +- sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this); +- +- sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); +- +- /* RT Scheduling setup */ +- // sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this); +- // sub_current_pose = nh.subscribe("/current_pose", 1, &TrajectoryGen::callbackGetCurrentPose, this); //origin 10 +- +- // int bVelSource = 1; +- // _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource); + // if(bVelSource == 0) +- // sub_robot_odom = nh.subscribe("/odom", 1, &TrajectoryGen::callbackGetRobotOdom, this); //origin 10 ++ // sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryGen::callbackGetRobotOdom, this); + // else if(bVelSource == 1) +- // sub_current_velocity = nh.subscribe("/current_velocity", 1, &TrajectoryGen::callbackGetVehicleStatus, this); //origin 10 ++ // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this); + // else if(bVelSource == 2) +- // sub_can_info = nh.subscribe("/can_info", 1, &TrajectoryGen::callbackGetCANInfo, this); //origin 10 ++ // sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this); + +- // sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); ++ ++ sub_pose_twist = nh.subscribe("/rubis_current_pose_twist", 1, &TrajectoryGen::callbackGetCurrentPoseTwist, this); // Def: 10 + } + + TrajectoryGen::~TrajectoryGen() +@@ -143,26 +130,45 @@ void TrajectoryGen::callbackGetInitPose(const geometry_msgs::PoseWithCovarianceS + } + } + +-void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +-{ +- m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) ++// { ++// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// m_InitPos = m_CurrentPos; ++// bNewCurrentPos = true; ++// bInitPos = true; ++// } ++ ++// void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) ++// { ++// m_VehicleStatus.speed = msg->twist.linear.x; ++// m_CurrentPos.v = m_VehicleStatus.speed; ++// if(fabs(msg->twist.linear.x) > 0.25) ++// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++// bVehicleStatus = true; ++// } ++ ++void TrajectoryGen::callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg){ ++ // Callback ++ rubis::instance_ = msg->instance; ++ ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); + m_InitPos = m_CurrentPos; + bNewCurrentPos = true; + bInitPos = true; +-} + +-void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) +-{ +- m_VehicleStatus.speed = msg->twist.linear.x; ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; + m_CurrentPos.v = m_VehicleStatus.speed; +- if(fabs(msg->twist.linear.x) > 0.25) +- m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); ++ current_pose_ = msg->pose; ++ current_twist_ = msg->twist; + } + ++ + void TrajectoryGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) + { + m_VehicleStatus.speed = msg->speed/3.6; +@@ -216,41 +222,34 @@ void TrajectoryGen::MainLoop() + { + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv"); + +- private_nh.param("/op_trajectory_generator/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_trajectory_generator/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/op_trajectory_generator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv"); +- private_nh.param("/op_trajectory_generator/rate", rate, 10); +- private_nh.param("/op_trajectory_generator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/op_trajectory_generator/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/op_trajectory_generator/task_relative_deadline", task_relative_deadline, (double)10); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); + + PlannerHNS::WayPoint prevState, state_change; + ++ ros::Rate r(rate); + +- ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(100); +- +- struct timespec start_time, end_time; +- +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ while(ros::ok()){ ++ rubis::start_task_profiling(); + + ros::spinOnce(); + +@@ -288,7 +287,7 @@ void TrajectoryGen::MainLoop() + -1 , -1, + m_RollOuts, sampledPoints_debug); + +- autoware_msgs::LaneArray local_lanes; ++ rubis_msgs::LaneArrayWithPoseTwist local_lanes; + for(unsigned int i=0; i < m_RollOuts.size(); i++) + { + for(unsigned int j=0; j < m_RollOuts.at(i).size(); j++) +@@ -301,28 +300,30 @@ void TrajectoryGen::MainLoop() + lane.cost = 0; + lane.is_blocked = false; + lane.lane_index = i; +- local_lanes.lanes.push_back(lane); ++ local_lanes.lane_array.lanes.push_back(lane); + } + } +- pub_LocalTrajectories.publish(local_lanes); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ ++ local_lanes.instance = rubis::instance_; ++ local_lanes.pose = current_pose_; ++ local_lanes.twist = current_twist_; ++ ++ pub_LocalTrajectoriesWithPoseTwist.publish(local_lanes); ++ // pub_LocalTrajectories.publish(local_lanes.lane_array); ++ + } +- else ++ else{ + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); + +- visualization_msgs::MarkerArray all_rollOuts; +- PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts); +- pub_LocalTrajectoriesRviz.publish(all_rollOuts); ++ visualization_msgs::MarkerArray all_rollOuts; ++ PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts); ++ pub_LocalTrajectoriesRviz.publish(all_rollOuts); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; ++ rubis::stop_task_profiling(0, 0); + } + +- loop_rate.sleep(); +- } ++ r.sleep(); ++ } + } + +-} ++} +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp +new file mode 100644 +index 00000000..2c40bb81 +--- /dev/null ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp +@@ -0,0 +1,315 @@ ++/* ++ * Copyright 2018-2019 Autoware Foundation. All rights reserved. ++ * ++ * Licensed under the Apache License, Version 2.0 (the "License"); ++ * you may not use this file except in compliance with the License. ++ * You may obtain a copy of the License at ++ * ++ * http://www.apache.org/licenses/LICENSE-2.0 ++ * ++ * Unless required by applicable law or agreed to in writing, software ++ * distributed under the License is distributed on an "AS IS" BASIS, ++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ++ * See the License for the specific language governing permissions and ++ * limitations under the License. ++ */ ++ ++#include "op_trajectory_generator_core.h" ++#include "op_ros_helpers/op_ROSHelpers.h" ++#include ++ ++#define SPIN_PROFILING ++ ++namespace TrajectoryGeneratorNS ++{ ++ ++TrajectoryGen::TrajectoryGen() ++{ ++ bInitPos = false; ++ bNewCurrentPos = false; ++ bVehicleStatus = false; ++ bWayGlobalPath = false; ++ ++ ros::NodeHandle _nh; ++ UpdatePlanningParams(_nh); ++ ++ tf::StampedTransform transform; ++ PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); ++ m_OriginPos.position.x = transform.getOrigin().x(); ++ m_OriginPos.position.y = transform.getOrigin().y(); ++ m_OriginPos.position.z = transform.getOrigin().z(); ++ ++ pub_LocalTrajectories = nh.advertise("local_trajectories", 1); ++ pub_LocalTrajectoriesWithPoseTwist = nh.advertise("local_trajectories_with_pose_twist", 1); ++ pub_LocalTrajectoriesRviz = nh.advertise("local_trajectories_gen_rviz", 1); ++ ++ sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this); ++ // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryGen::callbackGetCurrentPose, this); ++ ++ int bVelSource = 1; ++ _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource); ++ if(bVelSource == 0) ++ sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryGen::callbackGetRobotOdom, this); ++ // else if(bVelSource == 1) ++ // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this); ++ else if(bVelSource == 2) ++ sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this); ++ ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); ++ ++ sub_pose_twist = nh.subscribe("/rubis_current_pose_twist", 10, &TrajectoryGen::callbackGetCurrentPoseTwist, this); ++} ++ ++TrajectoryGen::~TrajectoryGen() ++{ ++} ++ ++void TrajectoryGen::UpdatePlanningParams(ros::NodeHandle& _nh) ++{ ++ _nh.getParam("/op_trajectory_generator/samplingTipMargin", m_PlanningParams.carTipMargin); ++ _nh.getParam("/op_trajectory_generator/samplingOutMargin", m_PlanningParams.rollInMargin); ++ _nh.getParam("/op_trajectory_generator/samplingSpeedFactor", m_PlanningParams.rollInSpeedFactor); ++ _nh.getParam("/op_trajectory_generator/enableHeadingSmoothing", m_PlanningParams.enableHeadingSmoothing); ++ ++ _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); ++ if(m_PlanningParams.enableSwerving) ++ m_PlanningParams.enableFollowing = true; ++ else ++ _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); ++ ++ _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); ++ _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); ++ ++ _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); ++ _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); ++ _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); ++ ++ _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); ++ _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); ++ if(m_PlanningParams.enableSwerving) ++ _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); ++ else ++ m_PlanningParams.rollOutNumber = 0; ++ ++ _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); ++ _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); ++ _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); ++ _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); ++ _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); ++ ++ _nh.getParam("/op_common_params/smoothingDataWeight", m_PlanningParams.smoothingDataWeight); ++ _nh.getParam("/op_common_params/smoothingSmoothWeight", m_PlanningParams.smoothingSmoothWeight); ++ ++ _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); ++ _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); ++ ++ _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); ++ ++ _nh.getParam("/op_common_params/width", m_CarInfo.width); ++ _nh.getParam("/op_common_params/length", m_CarInfo.length); ++ _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); ++ _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); ++ _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); ++ _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); ++ _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); ++ ++ m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; ++ m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; ++ ++} ++ ++void TrajectoryGen::callbackGetInitPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) ++{ ++ if(!bInitPos) ++ { ++ m_InitPos = PlannerHNS::WayPoint(msg->pose.pose.position.x+m_OriginPos.position.x, ++ msg->pose.pose.position.y+m_OriginPos.position.y, ++ msg->pose.pose.position.z+m_OriginPos.position.z, ++ tf::getYaw(msg->pose.pose.orientation)); ++ m_CurrentPos = m_InitPos; ++ bInitPos = true; ++ } ++} ++ ++// void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) ++// { ++// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// m_InitPos = m_CurrentPos; ++// bNewCurrentPos = true; ++// bInitPos = true; ++// } ++ ++// void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) ++// { ++// m_VehicleStatus.speed = msg->twist.linear.x; ++// m_CurrentPos.v = m_VehicleStatus.speed; ++// if(fabs(msg->twist.linear.x) > 0.25) ++// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++// bVehicleStatus = true; ++// } ++ ++void TrajectoryGen::callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg){ ++ // Before spinOnce ++ rubis::start_task_profiling(); ++ ++ // Callback ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ m_InitPos = m_CurrentPos; ++ bNewCurrentPos = true; ++ bInitPos = true; ++ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ ++ // After spinOnce ++ if(bInitPos && m_GlobalPaths.size()>0) ++ { ++ m_GlobalPathSections.clear(); ++ ++ for(unsigned int i = 0; i < m_GlobalPaths.size(); i++) ++ { ++ t_centerTrajectorySmoothed.clear(); ++ PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPaths.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , ++ m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); ++ ++ m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); ++ } ++ ++ std::vector sampledPoints_debug; ++ m_Planner.GenerateRunoffTrajectory(m_GlobalPathSections, m_CurrentPos, ++ m_PlanningParams.enableLaneChange, ++ m_VehicleStatus.speed, ++ m_PlanningParams.microPlanDistance, ++ m_PlanningParams.maxSpeed, ++ m_PlanningParams.minSpeed, ++ m_PlanningParams.carTipMargin, ++ m_PlanningParams.rollInMargin, ++ m_PlanningParams.rollInSpeedFactor, ++ m_PlanningParams.pathDensity, ++ m_PlanningParams.rollOutDensity, ++ m_PlanningParams.rollOutNumber, ++ m_PlanningParams.smoothingDataWeight, ++ m_PlanningParams.smoothingSmoothWeight, ++ m_PlanningParams.smoothingToleranceError, ++ m_PlanningParams.speedProfileFactor, ++ m_PlanningParams.enableHeadingSmoothing, ++ -1 , -1, ++ m_RollOuts, sampledPoints_debug); ++ ++ rubis_msgs::LaneArrayWithPoseTwist local_lanes; ++ for(unsigned int i=0; i < m_RollOuts.size(); i++) ++ { ++ for(unsigned int j=0; j < m_RollOuts.at(i).size(); j++) ++ { ++ autoware_msgs::Lane lane; ++ PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(m_RollOuts.at(i).at(j), m_CurrentPos, m_PlanningParams.minSpeed, m_PlanningParams.microPlanDistance); ++ PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_RollOuts.at(i).at(j), lane); ++ lane.closest_object_distance = 0; ++ lane.closest_object_velocity = 0; ++ lane.cost = 0; ++ lane.is_blocked = false; ++ lane.lane_index = i; ++ local_lanes.lane_array.lanes.push_back(lane); ++ } ++ } ++ ++ rubis::instance_ = msg->instance; ++ local_lanes.instance = rubis::instance_; ++ local_lanes.pose = msg->pose; ++ local_lanes.twist = msg->twist; ++ ++ pub_LocalTrajectoriesWithPoseTwist.publish(local_lanes); ++ pub_LocalTrajectories.publish(local_lanes.lane_array); ++ ++ } ++ else{ ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); ++ ++ visualization_msgs::MarkerArray all_rollOuts; ++ PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts); ++ pub_LocalTrajectoriesRviz.publish(all_rollOuts); ++ ++ rubis::stop_task_profiling(0, 0); ++ } ++} ++ ++ ++void TrajectoryGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) ++{ ++ m_VehicleStatus.speed = msg->speed/3.6; ++ m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void TrajectoryGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) ++{ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void TrajectoryGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++{ ++ if(msg->lanes.size() > 0) ++ { ++ bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); ++ ++ m_GlobalPaths.clear(); ++ ++ for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ { ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); ++ ++ PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); ++ m_GlobalPaths.push_back(m_temp_path); ++ ++ if(bOldGlobalPath) ++ { ++ bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); ++ } ++ } ++ ++ if(!bOldGlobalPath) ++ { ++ bWayGlobalPath = true; ++ std::cout << "Received New Global Path Generator ! " << std::endl; ++ } ++ else ++ { ++ m_GlobalPaths.clear(); ++ } ++ } ++} ++ ++void TrajectoryGen::MainLoop() ++{ ++ ros::NodeHandle private_nh("~"); ++ ++ // Scheduling Setup ++ std::string task_response_time_filename; ++ int rate; ++ double task_minimum_inter_release_time; ++ double task_execution_time; ++ double task_relative_deadline; ++ ++ private_nh.param("/op_trajectory_generator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv"); ++ private_nh.param("/op_trajectory_generator/rate", rate, 10); ++ private_nh.param("/op_trajectory_generator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); ++ private_nh.param("/op_trajectory_generator/task_execution_time", task_execution_time, (double)10); ++ private_nh.param("/op_trajectory_generator/task_relative_deadline", task_relative_deadline, (double)10); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ PlannerHNS::WayPoint prevState, state_change; ++ ++ ros::spin(); ++} ++ ++} +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt +index 97cc58e4..deef12e6 100644 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt +@@ -60,20 +60,4 @@ install( + install( + DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +-) +- +-if(CATKIN_ENABLE_TESTING) +- find_package(rostest REQUIRED) +- +- add_rostest_gtest( +- test-pure_pursuit +- test/test_pure_pursuit.test +- test/src/test_pure_pursuit.cpp +- src/pure_pursuit_core.cpp +- src/pure_pursuit.cpp src/pure_pursuit_viz.cpp +- ) +- add_dependencies(test-pure_pursuit ${catkin_EXPORTED_TARGETS}) +- target_link_libraries(test-pure_pursuit ${catkin_LIBRARIES}) +- +- roslint_add_test() +-endif() ++) +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h +index bab5c953..6a56fee1 100644 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h +@@ -24,6 +24,7 @@ + #include + #include + #include ++#include "autoware_msgs/Lane.h" + + // C++ includes + #include +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h +index 4e2511ef..3e6700e2 100755 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h +@@ -27,6 +27,7 @@ + #include + #include + #include ++#include + + #include + #include +@@ -44,6 +45,8 @@ + + #include + ++#include "rubis_msgs/LaneWithPoseTwist.h" ++ + #ifdef IONIC + #include + #endif +@@ -97,7 +100,7 @@ private: + pub11_, pub12_, pub13_, pub14_, pub15_, pub16_, pub17_, pub18_; + + // subscriber +- ros::Subscriber sub1_, pose_sub_, rubis_pose_sub_, sub3_, velocity_sub_, car_ctrl_output_sub; ++ ros::Subscriber sub1_, sub3_, pose_twist_sub_, final_waypoints_with_pose_twist_sub, car_ctrl_output_sub; + + // constant + const int LOOP_RATE_; // processing frequency +@@ -125,17 +128,14 @@ private: + // Added by PHY + bool dynamic_param_flag_; + std::vector dynamic_params; ++ autoware_msgs::Lane lane_; + + // callbacks +- void callbackFromConfig( +- const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config); +- void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); +- void callbackFromRubisCurrentPose(const rubis_msgs::PoseStampedConstPtr& _msg); ++ void callbackFromConfig(const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config); + void callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg); +- +- #ifdef SVL +- void callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg); +- #endif ++ void CallbackTwistPose(const rubis_msgs::PoseTwistStampedConstPtr& msg); ++ void CallbackFinalWaypointsWithPoseTwist(const rubis_msgs::LaneWithPoseTwistConstPtr& msg); ++ void _CallbackFinalWaypointsWithPoseTwist(); + + #ifdef IONIC + void callbackCtrlOutput(const can_data_msgs::Car_ctrl_output::ConstPtr &msg); +@@ -160,7 +160,7 @@ private: + const std::vector& waypoints) const; + void connectVirtualLastWaypoints( + autoware_msgs::Lane* expand_lane, LaneDirection direction); +- inline void updateCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); ++ inline void updateCurrentPose(geometry_msgs::PoseStampedConstPtr& msg); + + // Added by PHY + void setLookaheadParamsByVel(); +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch +index 359db355..0e4c9427 100644 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch +@@ -7,7 +7,6 @@ + + + +- + + + +@@ -15,7 +14,6 @@ + + + +- + + + +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch +index 3ac38797..e04e165e 100644 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch +@@ -7,7 +7,6 @@ + + + +- + + + +@@ -15,7 +14,6 @@ + + + +- + + + +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp +old mode 100755 +new mode 100644 +index 0532fe93..1e105f4a +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp +@@ -142,7 +142,6 @@ void PurePursuitNode::initForROS() + nh_.param("vehicle_info/wheel_base", wheel_base_, 2.7); + + private_nh_.param("/pure_pursuit/dynamic_params_flag", dynamic_param_flag_, false); +- private_nh_.param("/pure_pursuit/instance_mode", rubis::instance_mode_, 0); + + if(dynamic_param_flag_){ + XmlRpc::XmlRpcValue xml_list; +@@ -167,26 +166,15 @@ void PurePursuitNode::initForROS() + + + // setup subscriber +- sub1_ = nh_.subscribe("final_waypoints", 10, +- &PurePursuitNode::callbackFromWayPoints, this); +- +- if(rubis::instance_mode_) rubis_pose_sub_ = nh_.subscribe("rubis_current_pose", 10, &PurePursuitNode::callbackFromRubisCurrentPose, this); +- else pose_sub_ = nh_.subscribe("current_pose", 10, &PurePursuitNode::callbackFromCurrentPose, this); ++ pose_twist_sub_ = nh_.subscribe("/rubis_current_pose_twist", 1, &PurePursuitNode::CallbackTwistPose, this); ++ final_waypoints_with_pose_twist_sub = nh_.subscribe("/final_waypoints_with_pose_twist", 1, &PurePursuitNode::CallbackFinalWaypointsWithPoseTwist, this); // Def: 10 + + sub3_ = nh_.subscribe("config/waypoint_follower", 10, + &PurePursuitNode::callbackFromConfig, this); + +- #ifdef SVL +- velocity_sub_ = nh_.subscribe("current_velocity", 10, &PurePursuitNode::callbackFromCurrentVelocity, this); +- #endif +- +- #ifdef IONIC +- car_ctrl_output_sub = nh_.subscribe("car_ctrl_output", 10, &PurePursuitNode::callbackCtrlOutput, this); +- #endif +- + // setup publisher + twist_pub_ = nh_.advertise("twist_raw", 10); +- if(rubis::instance_mode_) rubis_twist_pub_ = nh_.advertise("rubis_twist_raw", 10); ++ rubis_twist_pub_ = nh_.advertise("rubis_twist_raw", 10); + + pub2_ = nh_.advertise("ctrl_raw", 10); + pub11_ = nh_.advertise("next_waypoint_mark", 0); +@@ -208,99 +196,32 @@ void PurePursuitNode::run() + { + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- +- private_nh.param("/pure_pursuit/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/pure_pursuit/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/pure_pursuit/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pure_pursuit.csv"); +- private_nh.param("/pure_pursuit/rate", rate, 10); +- private_nh.param("/pure_pursuit/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/pure_pursuit/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/pure_pursuit/task_relative_deadline", task_relative_deadline, (double)10); +- +- ROS_INFO_STREAM("pure pursuit start"); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- ros::Rate loop_rate(LOOP_RATE_); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(rate); +- +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- if (!is_pose_set_ || !is_waypoint_set_ || !is_velocity_set_) +- { +- // ROS_WARN("Necessary topics are not subscribed yet ... "); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pure_pursuit.csv"); + +- loop_rate.sleep(); +- continue; +- } +- +- pp_.setLookaheadDistance(computeLookaheadDistance()); +- pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_); +- +- double kappa = 0; +- bool can_get_curvature = pp_.canGetCurvature(&kappa); +- +- publishTwistStamped(can_get_curvature, kappa); +- publishControlCommandStamped(can_get_curvature, kappa); +- // for visualization with Rviz +- pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint())); +- pub13_.publish(displaySearchRadius( +- pp_.getCurrentPose().position, pp_.getLookaheadDistance())); +- pub12_.publish(displayNextTarget(pp_.getPoseOfNextTarget())); +- pub15_.publish(displayTrajectoryCircle( +- waypoint_follower::generateTrajectoryCircle( +- pp_.getPoseOfNextTarget(), pp_.getCurrentPose()))); +- if (add_virtual_end_waypoints_) +- { +- pub18_.publish( +- displayExpandWaypoints(pp_.getCurrentWaypoints(), expand_size_)); +- } +- std_msgs::Float32 angular_gravity_msg; +- angular_gravity_msg.data = +- computeAngularGravity(computeCommandVelocity(), kappa); +- pub16_.publish(angular_gravity_msg); +- +- publishDeviationCurrentPosition( +- pp_.getCurrentPose().position, pp_.getCurrentWaypoints()); +- +- is_pose_set_ = false; +- is_velocity_set_ = false; +- is_waypoint_set_ = false; +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- loop_rate.sleep(); +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + } + ++ + void PurePursuitNode::publishTwistStamped( + const bool& can_get_curvature, const double& kappa) const + { +@@ -310,15 +231,11 @@ void PurePursuitNode::publishTwistStamped( + ts.twist.angular.z = can_get_curvature ? kappa * ts.twist.linear.x : 0; + twist_pub_.publish(ts); + +- if(rubis::instance_mode_ && rubis::instance_mode_ != RUBIS_NO_INSTANCE){ +- rubis_msgs::TwistStamped rubis_ts; +- rubis_ts.instance = rubis::instance_; +- rubis_ts.msg = ts; +- rubis_twist_pub_.publish(rubis_ts); +- } +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ rubis_msgs::TwistStamped rubis_ts; ++ rubis_ts.instance = rubis::instance_; ++ rubis_ts.obj_instance = rubis::obj_instance_; ++ rubis_ts.msg = ts; ++ rubis_twist_pub_.publish(rubis_ts); + } + + void PurePursuitNode::publishControlCommandStamped( +@@ -395,6 +312,7 @@ double PurePursuitNode::computeCommandVelocity() const + + double PurePursuitNode::computeCommandAccel() const + { ++ if(pp_.getCurrentWaypoints().size() < 2) return 0.0; + const geometry_msgs::Pose current_pose = pp_.getCurrentPose(); + const geometry_msgs::Pose target_pose = + pp_.getCurrentWaypoints().at(1).pose.pose; +@@ -406,6 +324,7 @@ double PurePursuitNode::computeCommandAccel() const + const double v0 = current_linear_velocity_; + const double v = computeCommandVelocity(); + const double a = getSgn() * (v * v - v0 * v0) / (2 * x); ++ + return a; + } + +@@ -449,7 +368,7 @@ void PurePursuitNode::publishDeviationCurrentPosition( + pub17_.publish(msg); + } + +-inline void PurePursuitNode::updateCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg){ ++inline void PurePursuitNode::updateCurrentPose(geometry_msgs::PoseStampedConstPtr& msg){ + #ifndef USE_WAYPOINT_ORIENTATION + pp_.setCurrentPose(msg); + #else +@@ -462,27 +381,6 @@ inline void PurePursuitNode::updateCurrentPose(const geometry_msgs::PoseStampedC + is_pose_set_ = true; + } + +-void PurePursuitNode::callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +-{ +- updateCurrentPose(msg); +-} +- +-void PurePursuitNode::callbackFromRubisCurrentPose(const rubis_msgs::PoseStampedConstPtr& _msg) +-{ +- geometry_msgs::PoseStampedConstPtr msg = boost::make_shared(_msg->msg); +- rubis::instance_ = _msg->instance; +- updateCurrentPose(msg); +-} +- +-#ifdef SVL +-void PurePursuitNode::callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg) +-{ +- current_linear_velocity_ = msg->twist.linear.x; +- pp_.setCurrentVelocity(current_linear_velocity_); +- is_velocity_set_ = true; +-} +-#endif +- + #ifdef IONIC + void PurePursuitNode::callbackCtrlOutput(const can_data_msgs::Car_ctrl_output::ConstPtr &msg) + { +@@ -526,19 +424,32 @@ double PurePursuitNode::findWayPointVelocity(autoware_msgs::Waypoint msg){ + return way_points_velocity_[idx]; + } + +-void PurePursuitNode::callbackFromWayPoints( +- const autoware_msgs::LaneConstPtr& msg) ++void PurePursuitNode::CallbackTwistPose(const rubis_msgs::PoseTwistStampedConstPtr& msg) + { ++ rubis::start_task_profiling(); ++ rubis::instance_ = msg->instance; ++ ++ // Update pose ++ geometry_msgs::PoseStampedConstPtr pose_ptr(new geometry_msgs::PoseStamped(msg->pose)); ++ updateCurrentPose(pose_ptr); ++ ++ // Update twist ++ current_linear_velocity_ = msg->twist.twist.linear.x; ++ pp_.setCurrentVelocity(current_linear_velocity_); ++ is_velocity_set_ = true; ++ ++ if(lane_.waypoints.size() < 1) return; ++ + if(use_algorithm_){ +- command_linear_velocity_ = findWayPointVelocity(msg->waypoints.at(0)); ++ command_linear_velocity_ = findWayPointVelocity(lane_.waypoints.at(0)); + } + else{ +- command_linear_velocity_ = (!msg->waypoints.empty()) ? msg->waypoints.at(0).twist.twist.linear.x : 0; ++ command_linear_velocity_ = (!lane_.waypoints.empty()) ? lane_.waypoints.at(0).twist.twist.linear.x : 0; + } + +- geometry_msgs::Point curr_point = msg->waypoints.at(0).pose.pose.position; +- geometry_msgs::Point near_point = msg->waypoints.at(std::min(3, (int)msg->waypoints.size() - 1)).pose.pose.position; +- geometry_msgs::Point far_point = msg->waypoints.at(std::min(30, (int)msg->waypoints.size() - 1)).pose.pose.position; ++ geometry_msgs::Point curr_point = lane_.waypoints.at(0).pose.pose.position; ++ geometry_msgs::Point near_point = lane_.waypoints.at(std::min(3, (int)lane_.waypoints.size() - 1)).pose.pose.position; ++ geometry_msgs::Point far_point = lane_.waypoints.at(std::min(30, (int)lane_.waypoints.size() - 1)).pose.pose.position; + + double deg_1 = atan2((near_point.y - curr_point.y), (near_point.x - curr_point.x)) / 3.14 * 180; + double deg_2 = atan2((far_point.y - curr_point.y), (far_point.x - curr_point.x)) / 3.14 * 180; +@@ -549,15 +460,58 @@ void PurePursuitNode::callbackFromWayPoints( + + angle_diff_ = angle_diff; + ++ // Update waypoints ++ _CallbackFinalWaypointsWithPoseTwist(); ++ ++ // After spinOnce ++ pp_.setLookaheadDistance(computeLookaheadDistance()); ++ pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_); ++ ++ double kappa = 0; ++ bool can_get_curvature = pp_.canGetCurvature(&kappa); ++ ++ publishTwistStamped(can_get_curvature, kappa); ++ publishControlCommandStamped(can_get_curvature, kappa); ++ // for visualization with Rviz ++ pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint())); ++ pub13_.publish(displaySearchRadius( ++ pp_.getCurrentPose().position, pp_.getLookaheadDistance())); ++ pub12_.publish(displayNextTarget(pp_.getPoseOfNextTarget())); ++ pub15_.publish(displayTrajectoryCircle( ++ waypoint_follower::generateTrajectoryCircle( ++ pp_.getPoseOfNextTarget(), pp_.getCurrentPose()))); ++ if (add_virtual_end_waypoints_) ++ { ++ pub18_.publish( ++ displayExpandWaypoints(pp_.getCurrentWaypoints(), expand_size_)); ++ } ++ std_msgs::Float32 angular_gravity_msg; ++ angular_gravity_msg.data = ++ computeAngularGravity(computeCommandVelocity(), kappa); ++ pub16_.publish(angular_gravity_msg); ++ ++ publishDeviationCurrentPosition( ++ pp_.getCurrentPose().position, pp_.getCurrentWaypoints()); ++ ++ is_pose_set_ = false; ++ is_velocity_set_ = false; ++ is_waypoint_set_ = false; ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ ++} ++ ++void PurePursuitNode::_CallbackFinalWaypointsWithPoseTwist() ++{ + if(dynamic_param_flag_){ + setLookaheadParamsByVel(); + } + + if (add_virtual_end_waypoints_) + { +- const LaneDirection solved_dir = getLaneDirection(*msg); ++ const LaneDirection solved_dir = getLaneDirection(lane_); + direction_ = (solved_dir != LaneDirection::Error) ? solved_dir : direction_; +- autoware_msgs::Lane expanded_lane(*msg); ++ autoware_msgs::Lane expanded_lane(lane_); + expand_size_ = -expanded_lane.waypoints.size(); + connectVirtualLastWaypoints(&expanded_lane, direction_); + expand_size_ += expanded_lane.waypoints.size(); +@@ -566,15 +520,21 @@ void PurePursuitNode::callbackFromWayPoints( + } + else + { +- pp_.setCurrentWaypoints(msg->waypoints); ++ pp_.setCurrentWaypoints(lane_.waypoints); + } + is_waypoint_set_ = true; + + #ifdef USE_WAYPOINT_ORIENTATION +- waypoint_pose_ = msg->waypoints[0].pose; ++ waypoint_pose_ = lane_.waypoints[0].pose; + #endif + } + ++void PurePursuitNode::CallbackFinalWaypointsWithPoseTwist(const rubis_msgs::LaneWithPoseTwistConstPtr& msg) ++{ ++ rubis::obj_instance_ = msg->obj_instance; ++ lane_ = msg->lane; ++} ++ + void PurePursuitNode::connectVirtualLastWaypoints( + autoware_msgs::Lane* lane, LaneDirection direction) + { +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp b/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp +deleted file mode 100644 +index 5df8b48e..00000000 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp ++++ /dev/null +@@ -1,121 +0,0 @@ +-/* +- * Copyright 2018-2019 Autoware Foundation. All rights reserved. +- * +- * Licensed under the Apache License, Version 2.0 (the "License"); +- * you may not use this file except in compliance with the License. +- * You may obtain a copy of the License at +- * +- * http://www.apache.org/licenses/LICENSE-2.0 +- * +- * Unless required by applicable law or agreed to in writing, software +- * distributed under the License is distributed on an "AS IS" BASIS, +- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +- * See the License for the specific language governing permissions and +- * limitations under the License. +- */ +- +-#include +-#include +-#include +- +-namespace waypoint_follower +-{ +-class PurePursuitNodeTestSuite : public ::testing::Test +-{ +-protected: +- std::unique_ptr obj_; +- virtual void SetUp() +- { +- obj_ = std::unique_ptr(new PurePursuitNode()); +- obj_->add_virtual_end_waypoints_ = true; +- } +- virtual void TearDown() +- { +- obj_.reset(); +- } +- +-public: +- PurePursuitNodeTestSuite() {} +- ~PurePursuitNodeTestSuite() {} +- LaneDirection getDirection() +- { +- return obj_->direction_; +- } +- void ppCallbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg) +- { +- obj_->callbackFromWayPoints(msg); +- } +- void ppConnectVirtualLastWaypoints( +- autoware_msgs::Lane* expand_lane, LaneDirection direction) +- { +- obj_->connectVirtualLastWaypoints(expand_lane, direction); +- } +-}; +- +-TEST_F(PurePursuitNodeTestSuite, inputPositivePath) +-{ +- autoware_msgs::Lane original_lane; +- original_lane.waypoints.resize(3, autoware_msgs::Waypoint()); +- for (int i = 0; i < 3; i++) +- { +- original_lane.waypoints[i].pose.pose.position.x = i; +- original_lane.waypoints[i].pose.pose.orientation = +- tf::createQuaternionMsgFromYaw(0.0); +- } +- const autoware_msgs::LaneConstPtr +- lp(boost::make_shared(original_lane)); +- ppCallbackFromWayPoints(lp); +- ASSERT_EQ(getDirection(), LaneDirection::Forward) +- << "direction is not matching to positive lane."; +-} +- +-TEST_F(PurePursuitNodeTestSuite, inputNegativePath) +-{ +- autoware_msgs::Lane original_lane; +- original_lane.waypoints.resize(3, autoware_msgs::Waypoint()); +- for (int i = 0; i < 3; i++) +- { +- original_lane.waypoints[i].pose.pose.position.x = -i; +- original_lane.waypoints[i].pose.pose.orientation = +- tf::createQuaternionMsgFromYaw(0.0); +- } +- const autoware_msgs::LaneConstPtr +- lp(boost::make_shared(original_lane)); +- ppCallbackFromWayPoints(lp); +- ASSERT_EQ(getDirection(), LaneDirection::Backward) +- << "direction is not matching to negative lane."; +-} +-// If original lane is empty, new lane is also empty. +-TEST_F(PurePursuitNodeTestSuite, inputEmptyLane) +-{ +- autoware_msgs::Lane original_lane, new_lane; +- ppConnectVirtualLastWaypoints(&new_lane, LaneDirection::Forward); +- ASSERT_EQ(original_lane.waypoints.size(), new_lane.waypoints.size()) +- << "Input empty lane, and output is not empty"; +-} +- +-// If the original lane exceeds 2 points, +-// the additional part will be updated at +-// the interval of the first 2 points. +-TEST_F(PurePursuitNodeTestSuite, inputNormalLane) +-{ +- autoware_msgs::Lane original_lane; +- original_lane.waypoints.resize(2, autoware_msgs::Waypoint()); +- for (int i = 0; i < 2; i++) +- { +- original_lane.waypoints[i].pose.pose.position.x = i; +- } +- autoware_msgs::Lane new_lane(original_lane); +- ppConnectVirtualLastWaypoints(&new_lane, LaneDirection::Forward); +- +- ASSERT_LT(original_lane.waypoints.size(), new_lane.waypoints.size()) +- << "Fail to expand waypoints"; +-} +-} // namespace waypoint_follower +- +-int main(int argc, char** argv) +-{ +- testing::InitGoogleTest(&argc, argv); +- ros::init(argc, argv, "PurePursuitTest"); +- return RUN_ALL_TESTS(); +-} +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test b/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test +deleted file mode 100644 +index caba2948..00000000 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test ++++ /dev/null +@@ -1,3 +0,0 @@ +- +- +- +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h b/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h +index 997b984c..6c05ce54 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h +@@ -50,8 +50,9 @@ private: + ros::Subscriber twist_sub_, rubis_twist_sub_, ctrl_sub_, config_sub_; + // Added by PHY + ros::Subscriber emergency_stop_sub_; ++ autoware_msgs::ControlCommandStampedConstPtr ctrl_cmd_ptr_; + +- bool emergency_stop_; ++ bool emergency_stop_, current_emergency_stop_; + int max_stop_count_; + int current_stop_count_; + +@@ -60,13 +61,16 @@ private: + void rubisTwistCmdCallback(const rubis_msgs::TwistStampedConstPtr& _msg); + inline void publishTwist(const geometry_msgs::TwistStampedConstPtr& msg); + void ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg); ++ void _ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg); + void checkTwist(const twist_filter::Twist twist, const twist_filter::Twist twist_prev, const double& dt); + void checkCtrl(const twist_filter::Ctrl ctrl, const twist_filter::Ctrl ctrl_prev, const double& dt); + + //Added by PHY + void emergencyStopCallback(const std_msgs::Bool& msg); ++ void _emergencyStopCallback(); + }; + + } // namespace twist_filter_node + + #endif // TWIST_FILTER_TWIST_FILTER_NODE_H ++ +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch +index 81dc1fed..b3b8806e 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch +@@ -8,7 +8,6 @@ + + + +- + + + +@@ -24,12 +23,10 @@ + + + +- + + +- ++ + + +- + + +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch +index 0b470a12..b3b8806e 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch +@@ -8,7 +8,6 @@ + + + +- + + + +@@ -24,12 +23,10 @@ + + + +- + + +- ++ + + +- + + +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp +index 89165fd1..c447e46f 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp +@@ -29,27 +29,19 @@ TwistFilterNode::TwistFilterNode() : nh_(), private_nh_("~") + nh_.param("twist_filter/lowpass_gain_angular_z", twist_filter_config.lowpass_gain_angular_z, 0.0); + nh_.param("twist_filter/lowpass_gain_steering_angle", twist_filter_config.lowpass_gain_steering_angle, 0.0); + nh_.param("twist_filter/max_stop_count", max_stop_count_, 30); +- nh_.param("twist_filter/instance_mode", rubis::instance_mode_, 0); + twist_filter_ptr_ = std::make_shared(twist_filter_config); + emergency_stop_ = false; + current_stop_count_ = 0; + + // Subscribe +- if(rubis::instance_mode_) rubis_twist_sub_ = nh_.subscribe("rubis_twist_raw", 1, &TwistFilterNode::rubisTwistCmdCallback, this); +- else twist_sub_ = nh_.subscribe("twist_raw", 1, &TwistFilterNode::twistCmdCallback, this); ++ rubis_twist_sub_ = nh_.subscribe("rubis_twist_raw", 1, &TwistFilterNode::rubisTwistCmdCallback, this); + ctrl_sub_ = nh_.subscribe("ctrl_raw", 1, &TwistFilterNode::ctrlCmdCallback, this); + config_sub_ = nh_.subscribe("config/twist_filter", 10, &TwistFilterNode::configCallback, this); + emergency_stop_sub_ = nh_.subscribe("emergency_stop", 1 ,&TwistFilterNode::emergencyStopCallback, this); + +- /* RT Scheduling setup */ +- // twist_sub_ = nh_.subscribe("twist_raw", 1, &TwistFilterNode::twistCmdCallback, this); +- // ctrl_sub_ = nh_.subscribe("ctrl_raw", 1, &TwistFilterNode::ctrlCmdCallback, this); +- // config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistFilterNode::configCallback, this); //origin 10 +- // emergency_stop_sub_ = nh_.subscribe("emergency_stop", 1 ,&TwistFilterNode::emergencyStopCallback, this); +- + // Publish + twist_pub_ = nh_.advertise("twist_cmd", 5); +- if(rubis::instance_mode_) rubis_twist_pub_ = nh_.advertise("rubis_twist_cmd", 5); ++ rubis_twist_pub_ = nh_.advertise("rubis_twist_cmd", 5); + ctrl_pub_ = nh_.advertise("ctrl_cmd", 5); + twist_lacc_limit_debug_pub_ = private_nh_.advertise("limitation_debug/twist/lateral_accel", 5); + twist_ljerk_limit_debug_pub_ = private_nh_.advertise("limitation_debug/twist/lateral_jerk", 5); +@@ -113,6 +105,8 @@ inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConst + // Smoothed value publish + geometry_msgs::TwistStamped out_msg = *msg; + if(emergency_stop_ == false){ ++ // out_msg.twist.linear.x = twist_out.lx; ++ // out_msg.twist.angular.z = twist_out.az; + out_msg.twist.linear.x = twist_out.lx; + out_msg.twist.angular.z = twist_out.az; + } +@@ -121,15 +115,11 @@ inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConst + out_msg.twist.angular.z = 0; + } + twist_pub_.publish(out_msg); +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_msgs::TwistStamped rubis_out_msg; +- rubis_out_msg.instance = rubis::instance_; +- rubis_out_msg.msg = out_msg; +- rubis_twist_pub_.publish(rubis_out_msg); +- } +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ rubis_msgs::TwistStamped rubis_out_msg; ++ rubis_out_msg.instance = rubis::instance_; ++ rubis_out_msg.obj_instance = rubis::obj_instance_; ++ rubis_out_msg.msg = out_msg; ++ rubis_twist_pub_.publish(rubis_out_msg); + + // Publish lateral accel and jerk after smoothing + auto lacc_smoothed_result = twist_filter_ptr_->calcLaccWithAngularZ(twist_out); +@@ -153,20 +143,37 @@ inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConst + } + + void TwistFilterNode::rubisTwistCmdCallback(const rubis_msgs::TwistStampedConstPtr& _msg){ ++ // Before spin ++ rubis::start_task_profiling(); ++ ++ // Callback ++ _emergencyStopCallback(); ++ _ctrlCmdCallback(ctrl_cmd_ptr_); ++ + geometry_msgs::TwistStampedConstPtr msg = boost::make_shared(_msg-> msg); + rubis::instance_ = _msg->instance; ++ rubis::obj_instance_ = _msg->obj_instance; + publishTwist(msg); ++ ++ // After spin ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + + void TwistFilterNode::twistCmdCallback(const geometry_msgs::TwistStampedConstPtr& msg) + { +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + publishTwist(msg); + } + + void TwistFilterNode::ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg) + { ++ ctrl_cmd_ptr_ = boost::make_shared(*msg); ++} ++ ++void TwistFilterNode::_ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg) ++{ ++ if(ctrl_cmd_ptr_ == NULL) return; + const twist_filter::Ctrl ctrl = { msg->cmd.linear_velocity, msg->cmd.steering_angle }; + ros::Time current_time = ros::Time::now(); + +@@ -236,15 +243,19 @@ void TwistFilterNode::ctrlCmdCallback(const autoware_msgs::ControlCommandStamped + } + + void TwistFilterNode::emergencyStopCallback(const std_msgs::Bool& msg){ +- bool current_emergency_stop = msg.data; ++ current_emergency_stop_ = msg.data; ++ return; ++} ++ ++void TwistFilterNode::_emergencyStopCallback(){ + static std::string state("none"); + +- if(current_emergency_stop == true){ ++ if(current_emergency_stop_ == true){ + state = std::string("object is detected"); + emergency_stop_ = true; + current_stop_count_ = max_stop_count_; + } +- else if(current_emergency_stop == false && emergency_stop_ == true){ // Emergency Stop event is finished or wait ++ else if(current_emergency_stop_ == false && emergency_stop_ == true){ // Emergency Stop event is finished or wait + current_stop_count_--; + if(current_stop_count_ > 0){ + state = std::string("Wait for go"); +@@ -253,7 +264,7 @@ void TwistFilterNode::emergencyStopCallback(const std_msgs::Bool& msg){ + else + emergency_stop_ = false; + } +- else if(current_emergency_stop == false && emergency_stop_ == false){ // No event ++ else if(current_emergency_stop_ == false && emergency_stop_ == false){ // No event + state = std::string("No object"); + emergency_stop_ = false; + current_stop_count_ = 0; +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp +index eb82a63b..6a1da019 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp +@@ -17,67 +17,38 @@ + #include "twist_filter/twist_filter_node.h" + + extern unsigned long rubis::instance_; ++extern + + int main(int argc, char** argv) + { + ros::init(argc, argv, "twist_filter"); + twist_filter_node::TwistFilterNode node; + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- + ros::NodeHandle private_nh("~"); +- private_nh.param("/twist_filter/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/twist_filter/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/twist_filter/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_filter.csv"); +- private_nh.param("/twist_filter/rate", rate, 10); +- private_nh.param("/twist_filter/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/twist_filter/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/twist_filter/task_relative_deadline", task_relative_deadline, (double)10); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); + +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag){ +- rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- } ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); ++ std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_filter.csv"); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + + return 0; + } +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt +index 3073a8b7..6d0f645d 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt ++++ b/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt +@@ -44,17 +44,4 @@ install( + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +-) +- +-if(CATKIN_ENABLE_TESTING) +- roslint_add_test() +- find_package(rostest REQUIRED) +- add_rostest_gtest( +- test-twist_gate +- test/test_twist_gate.test +- test/src/test_twist_gate.cpp +- src/twist_gate.cpp +- ) +- add_dependencies(test-twist_gate ${catkin_EXPORTED_TARGETS}) +- target_link_libraries(test-twist_gate ${catkin_LIBRARIES}) +-endif() ++) +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h b/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h +index ba9dae2c..b6254f84 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h ++++ b/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h +@@ -60,7 +60,8 @@ private: + void watchdogTimer(); + void remoteCmdCallback(const remote_msgs_t::ConstPtr& input_msg); + void autoCmdTwistCmdCallback(const geometry_msgs::TwistStamped::ConstPtr& input_msg); +- void autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& _input_msg); ++ void autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg); ++ void _autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg); + void modeCmdCallback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg); + void gearCmdCallback(const tablet_socket_msgs::gear_cmd::ConstPtr& input_msg); + void accelCmdCallback(const autoware_msgs::AccelCmd::ConstPtr& input_msg); +@@ -89,6 +90,9 @@ private: + ros::Subscriber config_sub_; + std::map auto_cmd_sub_stdmap_; + ros::Timer timer_; ++ ros::Time current_time_; ++ ++ bool is_current_time_changed_; + + vehicle_cmd_msg_t twist_gate_msg_; + rubis_msgs::VehicleCmd rubis_twist_gate_msg_; +@@ -101,6 +105,8 @@ private: + std::thread watchdog_timer_thread_; + bool is_alive; + ++ rubis_msgs::TwistStamped::ConstPtr rubis_twist_cmd_ptr_; ++ + enum class CommandMode + { + AUTO = 1, +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp +index d008743b..e812acc9 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp ++++ b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp +@@ -45,32 +45,33 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n + , timeout_period_(10.0) + , command_mode_(CommandMode::AUTO) + , previous_command_mode_(CommandMode::AUTO) ++ , rubis_twist_cmd_ptr_(NULL) ++ , current_time_(0) ++ , is_current_time_changed_(false) + { + private_nh_.param("loop_rate", loop_rate_, 30.0); + private_nh_.param("use_decision_maker", use_decision_maker_, false); +- private_nh_.param("instance_mode", rubis::instance_mode_, 0); + + control_command_pub_ = nh_.advertise("/ctrl_mode", 1); + vehicle_cmd_pub_ = nh_.advertise("/vehicle_cmd", 1, true); +- if(rubis::instance_mode_) rubis_vehicle_cmd_pub_ = nh_.advertise("/rubis_vehicle_cmd", 1, true); +- remote_cmd_sub_ = nh_.subscribe("/remote_cmd", 1, &TwistGate::remoteCmdCallback, this); +- config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistGate::configCallback, this); +- ++ rubis_vehicle_cmd_pub_ = nh_.advertise("/rubis_vehicle_cmd", 1, true); ++ + timer_ = nh_.createTimer(ros::Duration(1.0 / loop_rate_), &TwistGate::timerCallback, this); + +- if(rubis::instance_mode_) auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/rubis_twist_cmd", 1, &TwistGate::autoCmdRubisTwistCmdCallback, this); +- else auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/twist_cmd", 1, &TwistGate::autoCmdTwistCmdCallback, this); ++ // remote_cmd_sub_ = nh_.subscribe("/remote_cmd", 1, &TwistGate::remoteCmdCallback, this); ++ config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistGate::configCallback, this); ++ auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/rubis_twist_cmd", 1, &TwistGate::autoCmdRubisTwistCmdCallback, this); + +- auto_cmd_sub_stdmap_["mode_cmd"] = nh_.subscribe("/mode_cmd", 1, &TwistGate::modeCmdCallback, this); +- auto_cmd_sub_stdmap_["gear_cmd"] = nh_.subscribe("/gear_cmd", 1, &TwistGate::gearCmdCallback, this); +- auto_cmd_sub_stdmap_["accel_cmd"] = nh_.subscribe("/accel_cmd", 1, &TwistGate::accelCmdCallback, this); +- auto_cmd_sub_stdmap_["steer_cmd"] = nh_.subscribe("/steer_cmd", 1, &TwistGate::steerCmdCallback, this); +- auto_cmd_sub_stdmap_["brake_cmd"] = nh_.subscribe("/brake_cmd", 1, &TwistGate::brakeCmdCallback, this); +- auto_cmd_sub_stdmap_["lamp_cmd"] = nh_.subscribe("/lamp_cmd", 1, &TwistGate::lampCmdCallback, this); +- auto_cmd_sub_stdmap_["ctrl_cmd"] = nh_.subscribe("/ctrl_cmd", 1, &TwistGate::ctrlCmdCallback, this); +- auto_cmd_sub_stdmap_["state"] = nh_.subscribe("/decision_maker/state", 1, &TwistGate::stateCallback, this); +- auto_cmd_sub_stdmap_["emergency_velocity"] = +- nh_.subscribe("emergency_velocity", 1, &TwistGate::emergencyCmdCallback, this); ++ // auto_cmd_sub_stdmap_["mode_cmd"] = nh_.subscribe("/mode_cmd", 1, &TwistGate::modeCmdCallback, this); ++ // auto_cmd_sub_stdmap_["gear_cmd"] = nh_.subscribe("/gear_cmd", 1, &TwistGate::gearCmdCallback, this); ++ // auto_cmd_sub_stdmap_["accel_cmd"] = nh_.subscribe("/accel_cmd", 1, &TwistGate::accelCmdCallback, this); ++ // auto_cmd_sub_stdmap_["steer_cmd"] = nh_.subscribe("/steer_cmd", 1, &TwistGate::steerCmdCallback, this); ++ // auto_cmd_sub_stdmap_["brake_cmd"] = nh_.subscribe("/brake_cmd", 1, &TwistGate::brakeCmdCallback, this); ++ // auto_cmd_sub_stdmap_["lamp_cmd"] = nh_.subscribe("/lamp_cmd", 1, &TwistGate::lampCmdCallback, this); ++ // auto_cmd_sub_stdmap_["ctrl_cmd"] = nh_.subscribe("/ctrl_cmd", 1, &TwistGate::ctrlCmdCallback, this); ++ // auto_cmd_sub_stdmap_["state"] = nh_.subscribe("/decision_maker/state", 1, &TwistGate::stateCallback, this); ++ // auto_cmd_sub_stdmap_["emergency_velocity"] = ++ // nh_.subscribe("emergency_velocity", 1, &TwistGate::emergencyCmdCallback, this); + + + twist_gate_msg_.header.seq = 0; +@@ -192,15 +193,26 @@ void TwistGate::remoteCmdCallback(const remote_msgs_t::ConstPtr& input_msg) + + void TwistGate::autoCmdTwistCmdCallback(const geometry_msgs::TwistStamped::ConstPtr& input_msg) + { +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + updateTwistGateMsg(input_msg); + } + +-void TwistGate::autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& _input_msg) ++void TwistGate::_autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg) + { +- geometry_msgs::TwistStamped::ConstPtr input_msg = boost::make_shared(_input_msg->msg); +- rubis::instance_ = _input_msg->instance; +- updateTwistGateMsg(input_msg); ++ if(rubis_twist_cmd_ptr_ == NULL) return; ++ if(!is_current_time_changed_){ ++ current_time_ = input_msg->msg.header.stamp; ++ is_current_time_changed_ = true; ++ } ++ rubis::instance_ = input_msg->instance; ++ rubis::obj_instance_ = input_msg->obj_instance; ++ geometry_msgs::TwistStamped::ConstPtr _input_msg = boost::make_shared(input_msg->msg); ++ updateTwistGateMsg(_input_msg); ++} ++ ++void TwistGate::autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg) ++{ ++ rubis_twist_cmd_ptr_ = boost::make_shared(*input_msg); + } + + inline void TwistGate::updateTwistGateMsg(const geometry_msgs::TwistStamped::ConstPtr& input_msg){ +@@ -211,14 +223,9 @@ inline void TwistGate::updateTwistGateMsg(const geometry_msgs::TwistStamped::Con + twist_gate_msg_.header.seq++; + twist_gate_msg_.twist_cmd.twist = input_msg->twist; + +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_twist_gate_msg_.instance = rubis::instance_; +- } ++ rubis_twist_gate_msg_.instance = rubis::instance_; + checkState(); + } +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; + } + + void TwistGate::modeCmdCallback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg) +@@ -347,15 +354,45 @@ void TwistGate::emergencyCmdCallback(const vehicle_cmd_msg_t::ConstPtr& input_ms + + void TwistGate::timerCallback(const ros::TimerEvent& e) + { ++ rubis::start_task_profiling(); ++ if(rubis_twist_cmd_ptr_ == NULL){ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ return; ++ } ++ ++ ++ static ros::Time previous_time = ros::Time::now(); ++ static double previous_target_velocity = 0.0; ++ static double previous_target_accel = 0.0; ++ ++ // Callback ++ _autoCmdRubisTwistCmdCallback(rubis_twist_cmd_ptr_); ++ ++ double diff_time = (current_time_ - previous_time).toSec(); ++ double current_target_velocity = 0.0, current_target_accel = 0.0; ++ ++ current_target_velocity = rubis_twist_cmd_ptr_->msg.twist.linear.x; ++ current_target_accel = (diff_time > 0) ? ((current_target_velocity - previous_target_velocity) / diff_time) : previous_target_accel; ++ + if(zero_flag_ == 1) + resetVehicleCmdMsg(); ++ twist_gate_msg_.ctrl_cmd.linear_acceleration = current_target_accel; ++ twist_gate_msg_.ctrl_cmd.linear_velocity = current_target_velocity; + + vehicle_cmd_pub_.publish(twist_gate_msg_); +- if(rubis::instance_mode_){ +- rubis_twist_gate_msg_.msg = twist_gate_msg_; +- rubis_vehicle_cmd_pub_.publish(rubis_twist_gate_msg_); ++ rubis_twist_gate_msg_.msg = twist_gate_msg_; ++ rubis_vehicle_cmd_pub_.publish(rubis_twist_gate_msg_); ++ ++ ++ ++ if(is_current_time_changed_){ ++ previous_time = current_time_; ++ previous_target_velocity = current_target_velocity; ++ previous_target_accel = current_target_accel; ++ is_current_time_changed_ = false; + } +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + void TwistGate::configCallback(const autoware_config_msgs::ConfigTwistFilter& msg) +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp +index e3f3e429..9e0bc827 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp ++++ b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp +@@ -28,61 +28,31 @@ int main(int argc, char** argv) + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- +- private_nh.param("/twist_gate/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/twist_gate/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/twist_gate/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_gate.csv"); +- private_nh.param("/twist_gate/rate", rate, 10); +- private_nh.param("/twist_gate/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/twist_gate/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/twist_gate/task_relative_deadline", task_relative_deadline, (double)10); +- private_nh.param("/twist_gate/zero_flag", zero_flag_, 0); +- + TwistGate twist_gate(nh, private_nh); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); ++ std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_gate.csv"); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + + return 0; + } +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp +deleted file mode 100644 +index cf8882a2..00000000 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp ++++ /dev/null +@@ -1,351 +0,0 @@ +-/* +- * Copyright 2019 Autoware Foundation. All rights reserved. +- * +- * Licensed under the Apache License, Version 2.0 (the "License"); +- * you may not use this file except in compliance with the License. +- * You may obtain a copy of the License at +- * +- * http://www.apache.org/licenses/LICENSE-2.0 +- * +- * Unless required by applicable law or agreed to in writing, software +- * distributed under the License is distributed on an "AS IS" BASIS, +- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +- * See the License for the specific language governing permissions and +- * limitations under the License. +- */ +- +-#include +-#include +- +-#include "test_twist_gate.hpp" +- +-class TwistGateTestSuite : public ::testing::Test +-{ +-public: +- TwistGateTestSuite() {} +- ~TwistGateTestSuite() {} +- +- TwistGateTestClass test_obj_; +- +-protected: +- virtual void SetUp() +- { +- ros::NodeHandle nh; +- ros::NodeHandle private_nh("~"); +- test_obj_.tg = new TwistGate(nh, private_nh); +- }; +- virtual void TearDown() { delete test_obj_.tg; } +-}; +- +-TEST_F(TwistGateTestSuite, resetVehicelCmd) +-{ +- double d_value = 1.5; +- int i_value = 1; +- +- autoware_msgs::VehicleCmd msg = test_obj_.setTgTwistGateMsg(d_value, i_value); +- +- ASSERT_EQ(d_value, msg.twist_cmd.twist.linear.x); +- ASSERT_EQ(d_value, msg.twist_cmd.twist.angular.z); +- ASSERT_EQ(i_value, msg.mode); +- ASSERT_EQ(i_value, msg.gear_cmd.gear); +- ASSERT_EQ(i_value, msg.lamp_cmd.l); +- ASSERT_EQ(i_value, msg.lamp_cmd.r); +- ASSERT_EQ(i_value, msg.accel_cmd.accel); +- ASSERT_EQ(i_value, msg.brake_cmd.brake); +- ASSERT_EQ(i_value, msg.steer_cmd.steer); +- ASSERT_EQ(i_value, msg.ctrl_cmd.linear_velocity); +- ASSERT_EQ(i_value, msg.ctrl_cmd.steering_angle); +- ASSERT_EQ(i_value, msg.emergency); +- +- test_obj_.tgResetVehicleCmdMsg(); +- msg = test_obj_.getTgTwistGateMsg(); +- +- ASSERT_EQ(0, msg.twist_cmd.twist.linear.x); +- ASSERT_EQ(0, msg.twist_cmd.twist.angular.z); +- ASSERT_EQ(0, msg.mode); +- ASSERT_EQ(0, msg.gear_cmd.gear); +- ASSERT_EQ(0, msg.lamp_cmd.l); +- ASSERT_EQ(0, msg.lamp_cmd.r); +- ASSERT_EQ(0, msg.accel_cmd.accel); +- ASSERT_EQ(0, msg.brake_cmd.brake); +- ASSERT_EQ(0, msg.steer_cmd.steer); +- ASSERT_EQ(-1, msg.ctrl_cmd.linear_velocity); +- ASSERT_EQ(0, msg.ctrl_cmd.steering_angle); +- ASSERT_EQ(0, msg.emergency); +-} +- +-TEST_F(TwistGateTestSuite, twistCmdCallback) +-{ +- double linear_x = 5.0; +- double angular_z = 1.5; +- test_obj_.publishTwistCmd(linear_x, angular_z); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(linear_x, test_obj_.cb_vehicle_cmd.twist_cmd.twist.linear.x); +- ASSERT_EQ(angular_z, test_obj_.cb_vehicle_cmd.twist_cmd.twist.angular.z); +-} +- +-TEST_F(TwistGateTestSuite, controlCmdCallback) +-{ +- double linear_vel = 5.0; +- double linear_acc = 1.5; +- double steer_angle = 1.57; +- test_obj_.publishControlCmd(linear_vel, linear_acc, steer_angle); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(linear_vel, test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_velocity); +- ASSERT_EQ(linear_acc, test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_acceleration); +- ASSERT_EQ(steer_angle, test_obj_.cb_vehicle_cmd.ctrl_cmd.steering_angle); +-} +- +-TEST_F(TwistGateTestSuite, remoteCmdCallback) +-{ +- autoware_msgs::RemoteCmd remote_cmd; +- remote_cmd.vehicle_cmd.header.frame_id = "/test_frame"; +- remote_cmd.vehicle_cmd.header.stamp = ros::Time::now(); +- remote_cmd.vehicle_cmd.twist_cmd.twist.linear.x = 5.0; +- remote_cmd.vehicle_cmd.twist_cmd.twist.angular.z = 0.785; +- remote_cmd.vehicle_cmd.ctrl_cmd.linear_velocity = 4.0; +- remote_cmd.vehicle_cmd.ctrl_cmd.linear_acceleration = 3.0; +- remote_cmd.vehicle_cmd.ctrl_cmd.steering_angle = 0.393; +- remote_cmd.vehicle_cmd.accel_cmd.accel = 10; +- remote_cmd.vehicle_cmd.brake_cmd.brake = 20; +- remote_cmd.vehicle_cmd.steer_cmd.steer = 30; +- remote_cmd.vehicle_cmd.gear_cmd.gear = autoware_msgs::Gear::PARK; +- remote_cmd.vehicle_cmd.lamp_cmd.l = 1; +- remote_cmd.vehicle_cmd.lamp_cmd.r = 1; +- remote_cmd.vehicle_cmd.mode = 6; +- remote_cmd.vehicle_cmd.emergency = 0; +- remote_cmd.control_mode = 2; +- +- test_obj_.publishRemoteCmd(remote_cmd); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(remote_cmd.vehicle_cmd.header.frame_id +- , test_obj_.cb_vehicle_cmd.header.frame_id); +- ASSERT_EQ(remote_cmd.vehicle_cmd.header.stamp +- , test_obj_.cb_vehicle_cmd.header.stamp); +- ASSERT_EQ(remote_cmd.vehicle_cmd.twist_cmd.twist.linear.x +- , test_obj_.cb_vehicle_cmd.twist_cmd.twist.linear.x); +- ASSERT_EQ(remote_cmd.vehicle_cmd.twist_cmd.twist.angular.z +- , test_obj_.cb_vehicle_cmd.twist_cmd.twist.angular.z); +- ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.linear_velocity +- , test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_velocity); +- ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.linear_acceleration +- , test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_acceleration); +- ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.steering_angle +- , test_obj_.cb_vehicle_cmd.ctrl_cmd.steering_angle); +- ASSERT_EQ(remote_cmd.vehicle_cmd.accel_cmd.accel +- , test_obj_.cb_vehicle_cmd.accel_cmd.accel); +- ASSERT_EQ(remote_cmd.vehicle_cmd.brake_cmd.brake +- , test_obj_.cb_vehicle_cmd.brake_cmd.brake); +- ASSERT_EQ(remote_cmd.vehicle_cmd.steer_cmd.steer +- , test_obj_.cb_vehicle_cmd.steer_cmd.steer); +- ASSERT_EQ(remote_cmd.vehicle_cmd.gear_cmd.gear +- , test_obj_.cb_vehicle_cmd.gear_cmd.gear); +- ASSERT_EQ(remote_cmd.vehicle_cmd.lamp_cmd.l +- , test_obj_.cb_vehicle_cmd.lamp_cmd.l); +- ASSERT_EQ(remote_cmd.vehicle_cmd.lamp_cmd.r +- , test_obj_.cb_vehicle_cmd.lamp_cmd.r); +- ASSERT_EQ(remote_cmd.vehicle_cmd.mode +- , test_obj_.cb_vehicle_cmd.mode); +-} +- +-TEST_F(TwistGateTestSuite, modeCmdCallback) +-{ +- int mode = 8; +- +- test_obj_.publishModeCmd(mode); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(mode, test_obj_.cb_vehicle_cmd.mode); +-} +- +-TEST_F(TwistGateTestSuite, gearCmdCallback) +-{ +- int gear = autoware_msgs::Gear::DRIVE; +- +- test_obj_.publishGearCmd(gear); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(gear, test_obj_.cb_vehicle_cmd.gear_cmd.gear); +-} +- +-TEST_F(TwistGateTestSuite, accelCmdCallback) +-{ +- int accel = 100; +- +- test_obj_.publishAccelCmd(accel); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(accel, test_obj_.cb_vehicle_cmd.accel_cmd.accel); +-} +- +-TEST_F(TwistGateTestSuite, steerCmdCallback) +-{ +- int steer = 100; +- +- test_obj_.publishSteerCmd(steer); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(steer, test_obj_.cb_vehicle_cmd.steer_cmd.steer); +-} +- +-TEST_F(TwistGateTestSuite, brakeCmdCallback) +-{ +- int brake = 100; +- +- test_obj_.publishBrakeCmd(brake); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(brake, test_obj_.cb_vehicle_cmd.brake_cmd.brake); +-} +- +-TEST_F(TwistGateTestSuite, lampCmdCallback) +-{ +- int lamp_l = 1; +- int lamp_r = 1; +- +- test_obj_.publishLampCmd(lamp_l, lamp_r); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(lamp_l, test_obj_.cb_vehicle_cmd.lamp_cmd.l); +- ASSERT_EQ(lamp_r, test_obj_.cb_vehicle_cmd.lamp_cmd.r); +-} +- +-TEST_F(TwistGateTestSuite, emergencyVehicleCmdCallback) +-{ +- int lamp_l_pre = 0; +- int lamp_r_pre = 0; +- int emergency = 1; +- +- test_obj_.publishLampCmd(lamp_l_pre, lamp_r_pre); +- test_obj_.publishEmergencyVehicleCmd(emergency); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(emergency, test_obj_.cb_vehicle_cmd.emergency); +- +- int lamp_l = 1; +- int lamp_r = 1; +- test_obj_.publishLampCmd(lamp_l, lamp_r); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(lamp_l_pre, test_obj_.cb_vehicle_cmd.lamp_cmd.l); +- ASSERT_EQ(lamp_r_pre, test_obj_.cb_vehicle_cmd.lamp_cmd.r); +- +- std::this_thread::sleep_for(std::chrono::milliseconds(10000)); +- +- test_obj_.publishLampCmd(lamp_l, lamp_r); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(lamp_l, test_obj_.cb_vehicle_cmd.lamp_cmd.l); +- ASSERT_EQ(lamp_r, test_obj_.cb_vehicle_cmd.lamp_cmd.r); +-} +- +-TEST_F(TwistGateTestSuite, stateCallback) +-{ +- test_obj_.publishDecisionMakerState("VehicleReady\nWaitOrder\nWaitEngage\n"); +- test_obj_.tgSpinOnce(); +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- +- autoware_msgs::VehicleCmd tg_msg = test_obj_.getTwistGateMsg(); +- ASSERT_EQ(autoware_msgs::Gear::PARK, tg_msg.gear_cmd.gear); +- ASSERT_EQ(false, test_obj_.getIsStateDriveFlag()); +- +- test_obj_.publishDecisionMakerState("VehicleReady\nDriving\nDrive\n"); +- test_obj_.tgSpinOnce(); +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- +- tg_msg = test_obj_.getTwistGateMsg(); +- ASSERT_EQ(autoware_msgs::Gear::DRIVE, tg_msg.gear_cmd.gear); +- ASSERT_EQ(true, test_obj_.getIsStateDriveFlag()); +-} +- +-int main(int argc, char **argv) +-{ +- testing::InitGoogleTest(&argc, argv); +- ros::init(argc, argv, "TwistGateTestNode"); +- return RUN_ALL_TESTS(); +-} +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp b/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp +deleted file mode 100644 +index bd4acca1..00000000 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp ++++ /dev/null +@@ -1,168 +0,0 @@ +-/* +- * Copyright 2019 Autoware Foundation. All rights reserved. +- * +- * Licensed under the Apache License, Version 2.0 (the "License"); +- * you may not use this file except in compliance with the License. +- * You may obtain a copy of the License at +- * +- * http://www.apache.org/licenses/LICENSE-2.0 +- * +- * Unless required by applicable law or agreed to in writing, software +- * distributed under the License is distributed on an "AS IS" BASIS, +- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +- * See the License for the specific language governing permissions and +- * limitations under the License. +- */ +- +-#include +-#include +- +-#include "twist_gate/twist_gate.h" +- +-class TwistGateTestClass { +-public: +- TwistGateTestClass() { +- twist_cmd_publisher = +- nh.advertise("twist_cmd", 0); +- control_cmd_publisher = +- nh.advertise("ctrl_cmd", 0); +- decision_maker_state_publisher = +- nh.advertise("decision_maker/state", 0); +- remote_cmd_publisher = nh.advertise("remote_cmd", 0); +- mode_cmd_publisher = nh.advertise("mode_cmd", 0); +- gear_cmd_publisher = nh.advertise("gear_cmd", 0); +- accel_cmd_publisher = nh.advertise("accel_cmd", 0); +- steer_cmd_publisher = nh.advertise("steer_cmd", 0); +- brake_cmd_publisher = nh.advertise("brake_cmd", 0); +- lamp_cmd_publisher = nh.advertise("lamp_cmd", 0); +- emergency_vehicle_cmd_publisher = nh.advertise("emergency_velocity", 0); +- vehicle_cmd_subscriber = nh.subscribe( +- "/vehicle_cmd", 1, &TwistGateTestClass::vehicleCmdCallback, this); +- } +- +- TwistGate *tg; +- autoware_msgs::VehicleCmd cb_vehicle_cmd; +- +- ros::NodeHandle nh; +- ros::Publisher twist_cmd_publisher, control_cmd_publisher, remote_cmd_publisher, mode_cmd_publisher, gear_cmd_publisher, accel_cmd_publisher, steer_cmd_publisher, brake_cmd_publisher, lamp_cmd_publisher, decision_maker_state_publisher, emergency_vehicle_cmd_publisher; +- ros::Subscriber vehicle_cmd_subscriber; +- +- void tgSpinOnce() { tg->spinOnce(); } +- +- void tgResetVehicleCmdMsg() { tg->resetVehicleCmdMsg(); } +- +- autoware_msgs::VehicleCmd setTgTwistGateMsg(double d_value, int i_value) { +- tg->twist_gate_msg_.twist_cmd.twist.linear.x = d_value; +- tg->twist_gate_msg_.twist_cmd.twist.angular.z = d_value; +- tg->twist_gate_msg_.mode = i_value; +- tg->twist_gate_msg_.gear_cmd.gear = i_value; +- tg->twist_gate_msg_.lamp_cmd.l = i_value; +- tg->twist_gate_msg_.lamp_cmd.r = i_value; +- tg->twist_gate_msg_.accel_cmd.accel = i_value; +- tg->twist_gate_msg_.brake_cmd.brake = i_value; +- tg->twist_gate_msg_.steer_cmd.steer = i_value; +- tg->twist_gate_msg_.ctrl_cmd.linear_velocity = i_value; +- tg->twist_gate_msg_.ctrl_cmd.steering_angle = i_value; +- tg->twist_gate_msg_.emergency = i_value; +- +- return tg->twist_gate_msg_; +- } +- +- autoware_msgs::VehicleCmd getTgTwistGateMsg() {return tg->twist_gate_msg_;} +- +- void publishTwistCmd(double linear_x, double angular_z) { +- geometry_msgs::TwistStamped msg; +- msg.header.stamp = ros::Time::now(); +- msg.twist.linear.x = linear_x; +- msg.twist.angular.z = angular_z; +- +- twist_cmd_publisher.publish(msg); +- } +- +- void publishControlCmd(double linear_vel, double linear_acc, +- double steer_angle) { +- autoware_msgs::ControlCommandStamped msg; +- msg.header.stamp = ros::Time::now(); +- msg.cmd.linear_velocity = linear_vel; +- msg.cmd.linear_acceleration = linear_acc; +- msg.cmd.steering_angle = steer_angle; +- +- control_cmd_publisher.publish(msg); +- } +- +- void publishRemoteCmd(autoware_msgs::RemoteCmd remote_cmd){ +- remote_cmd_publisher.publish(remote_cmd); +- } +- +- void publishModeCmd(int mode){ +- tablet_socket_msgs::mode_cmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.mode = mode; +- +- mode_cmd_publisher.publish(msg); +- } +- +- void publishGearCmd(int gear){ +- tablet_socket_msgs::gear_cmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.gear = gear; +- +- gear_cmd_publisher.publish(msg); +- } +- +- void publishAccelCmd(int accel){ +- autoware_msgs::AccelCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.accel = accel; +- +- accel_cmd_publisher.publish(msg); +- } +- +- void publishSteerCmd(int steer){ +- autoware_msgs::SteerCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.steer = steer; +- +- steer_cmd_publisher.publish(msg); +- } +- +- void publishBrakeCmd(int brake){ +- autoware_msgs::BrakeCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.brake = brake; +- +- brake_cmd_publisher.publish(msg); +- } +- +- void publishLampCmd(int lamp_l, int lamp_r){ +- autoware_msgs::LampCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.l = lamp_l; +- msg.r = lamp_r; +- +- lamp_cmd_publisher.publish(msg); +- } +- +- void publishEmergencyVehicleCmd(int emergency){ +- autoware_msgs::VehicleCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.emergency = emergency; +- +- emergency_vehicle_cmd_publisher.publish(msg); +- } +- +- void publishDecisionMakerState(std::string states) { +- std_msgs::String msg; +- msg.data = states; +- +- decision_maker_state_publisher.publish(msg); +- } +- +- void vehicleCmdCallback(autoware_msgs::VehicleCmd msg) { +- cb_vehicle_cmd = msg; +- } +- +- autoware_msgs::VehicleCmd getTwistGateMsg() { return tg->twist_gate_msg_; } +- +- bool getIsStateDriveFlag() { return tg->is_state_drive_; } +-}; +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test b/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test +deleted file mode 100644 +index abe4934a..00000000 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test ++++ /dev/null +@@ -1,5 +0,0 @@ +- +- +- +- +- +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst b/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst +new file mode 100644 +index 00000000..41c70f27 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst +@@ -0,0 +1,267 @@ ++^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ++Changelog for package nav_msgs ++^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ++ ++1.13.1 (2021-01-11) ++------------------- ++* Update package maintainers (`#168 `_) ++* Add LoadMap service (`#164 `_) ++* Contributors: David V. Lu!!, Michel Hidalgo ++ ++1.13.0 (2020-05-21) ++------------------- ++* Bump CMake version to avoid CMP0048 warning (`#158 `_) ++* Contributors: Shane Loretz ++ ++1.12.7 (2018-11-06) ++------------------- ++ ++1.12.6 (2018-05-03) ++------------------- ++ ++1.12.5 (2016-09-30) ++------------------- ++ ++1.12.4 (2016-02-22) ++------------------- ++ ++1.12.3 (2015-04-20) ++------------------- ++ ++1.12.2 (2015-03-21) ++------------------- ++* change type of initial_pose in SetMap service to PoseWithCovarianceStamped ++* Contributors: Stephan Wirth ++ ++1.12.1 (2015-03-17) ++------------------- ++* updating outdated urls. fixes `#52 `_. ++* Adds a SetMap service message to support swap maps functionality in amcl ++* Contributors: Tully Foote, liz-murphy ++ ++1.12.0 (2014-12-29) ++------------------- ++ ++1.11.6 (2014-11-04) ++------------------- ++ ++1.11.5 (2014-10-27) ++------------------- ++ ++1.11.4 (2014-06-19) ++------------------- ++ ++1.11.3 (2014-05-07) ++------------------- ++* Export architecture_independent flag in package.xml ++* Contributors: Scott K Logan ++ ++1.11.2 (2014-04-24) ++------------------- ++ ++1.11.1 (2014-04-16) ++------------------- ++ ++1.11.0 (2014-03-04) ++------------------- ++ ++1.10.6 (2014-02-27) ++------------------- ++ ++1.10.5 (2014-02-25) ++------------------- ++ ++1.10.4 (2014-02-18) ++------------------- ++ ++1.10.3 (2014-01-07) ++------------------- ++ ++1.10.2 (2013-08-19) ++------------------- ++ ++1.10.1 (2013-08-16) ++------------------- ++ ++1.10.0 (2013-07-13) ++------------------- ++ ++1.9.16 (2013-05-21) ++------------------- ++* added action definition for getting maps ++* update email in package.xml ++ ++1.9.15 (2013-03-08) ++------------------- ++ ++1.9.14 (2013-01-19) ++------------------- ++ ++1.9.13 (2013-01-13) ++------------------- ++ ++1.9.12 (2013-01-02) ++------------------- ++ ++1.9.11 (2012-12-17) ++------------------- ++* modified dep type of catkin ++ ++1.9.10 (2012-12-13) ++------------------- ++* add missing downstream depend ++* switched from langs to message_* packages ++ ++1.9.9 (2012-11-22) ++------------------ ++ ++1.9.8 (2012-11-14) ++------------------ ++ ++1.9.7 (2012-10-30) ++------------------ ++* fix catkin function order ++ ++1.9.6 (2012-10-18) ++------------------ ++* updated cmake min version to 2.8.3, use cmake_parse_arguments instead of custom macro ++ ++1.9.5 (2012-09-28) ++------------------ ++ ++1.9.4 (2012-09-27 18:06) ++------------------------ ++ ++1.9.3 (2012-09-27 17:39) ++------------------------ ++* cleanup ++* cleaned up package.xml files ++* updated to latest catkin ++* fixed dependencies and more ++* updated to latest catkin: created package.xmls, updated CmakeLists.txt ++ ++1.9.2 (2012-09-05) ++------------------ ++* updated pkg-config in manifest.xml ++ ++1.9.1 (2012-09-04) ++------------------ ++* use install destination variables, removed manual installation of manifests ++ ++1.9.0 (2012-08-29) ++------------------ ++* updated to current catkin ++ ++1.8.13 (2012-07-26 18:34:15 +0000) ++---------------------------------- ++ ++1.8.8 (2012-06-12 22:36) ++------------------------ ++* make find_package REQUIRED ++* removed obsolete catkin tag from manifest files ++* fixed package dependencies for several common messages (fixed `#3956 `_) ++* adding manifest exports ++* removed depend, added catkin ++* stripping depend and export tags from common_msgs manifests as msg dependencies are now declared in cmake and stack.yaml. Also removed bag migration exports ++* common_msgs: removing migration rules as all are over a year old ++* bye bye vestigial MSG_DIRS ++* nav_msgs: getting rid of other build files and cleaning up ++* common_msgs: starting catkin conversion ++* adios rosbuild2 in manifest.xml ++* catkin updates ++* catkin_project ++* Updated to work with new message generation macros ++* More tweaking for standalone message generation ++* Getting standalone message generation working... w/o munging rosbuild2 ++* more rosbuild2 hacking ++* missing dependencies ++* updating bagmigration exports ++* rosbuild2 taking shape ++* removing old exports ros`#2292 `_ ++* Added Ubuntu platform tags to manifest ++* Adding a start pose to the GetPlan service ++* Remove use of deprecated rosbuild macros ++* Fixing migration rules for nav_msgs. ++* Changed byte to int8, in response to map_server doc review ++* changing review status ++* adding documentation for `#2997 `_ ++* removing redundant range statements as per ticket:2997 ++* Adding documentation to the Odometry message to make things more clear ++* manifest update ++* updated description and url ++* full migration rules ++* adding child_frame_id as per discussion about odometry message ++* Adding a header to Path ++* Adding a header to the GridCells message ++* Adding a new GridCells message for displaying obstacles in nav_view and rviz ++* clearing API reviews for they've been through a bunch of them recently. ++* fixing stack name ++* Adding comments to path ++* documenting messages ++* Making odometry migration fail until we have worked out appropriate way to handle covariances. ++* Changing naming of bag migration rules. ++* Modifying migration rules for Odometry and WrenchStamped change of field names. ++* Adding actual migration rules for all of the tested common_msgs migrations. ++* `#2250 `_ getting rid of _with_covariance in Odometry fields ++* nav_msgs: added missing srv export ++* Adding migration rules to get migration tests to pass. ++* removing last of robot_msgs and all dependencies on it ++* moving Path from robot_msgs to nav_msgs `#2281 `_ ++* adding header to OccupancyGrid `#1906 `_ ++* First half of the change from deprecated_msgs::RobotBase2DOdom to nav_msgs::Odometry, I think all the c++ compiles, can't speak for functionality yet, also... the python has yet to be run... this may break some things ++* moving PoseArray into geometry_msgs `#1907 `_ ++* fixing names ++* Removing header since there's already one in the pose and fixing message definition to have variable names ++* adding Odometry message as per API review and ticket:2250 ++* merging in the changes to messages see ros-users email. THis is about half the common_msgs API changes ++* Forgot to check in the services I added.... shoot ++* Moving StaticMap.srv to GetMap.srv and moving Plan.srv to GetPlan.srv, also moving them to nav_msgs and removing the nav_srvs package ++* Merging tha actionlib branch back into trunk ++ r29135@att (orig r19792): eitanme | 2009-07-27 18:30:30 -0700 ++ Creating a branch for actionlib.... hopefully for the last time ++ r29137@att (orig r19794): eitanme | 2009-07-27 18:32:49 -0700 ++ Changing ParticleCloud to PoseArray ++ r29139@att (orig r19796): eitanme | 2009-07-27 18:33:42 -0700 ++ Adding action definition to the rep ++ r29148@att (orig r19805): eitanme | 2009-07-27 18:47:39 -0700 ++ Some fixes... almost compiling ++ r29165@att (orig r19822): eitanme | 2009-07-27 20:41:07 -0700 ++ Macro version of the typedefs that compiles ++ r29213@att (orig r19869): eitanme | 2009-07-28 11:49:10 -0700 ++ Compling version of the ActionServer re-write complete with garbage collection, be default it will keep goals without goal handles for 5 seconds ++ r29220@att (orig r19876): eitanme | 2009-07-28 12:06:06 -0700 ++ Fix to make sure that transitions into preempting and recalling states actually happen ++ r29254@att (orig r19888): eitanme | 2009-07-28 13:27:40 -0700 ++ Forgot to actually call the cancel callback... addind a subscriber on the cancel topic ++ r29267@att (orig r19901): eitanme | 2009-07-28 14:41:09 -0700 ++ Adding text field to GoalStatus to allow users to throw some debugging information into the GoalStatus messages ++ r29275@att (orig r19909): eitanme | 2009-07-28 15:43:49 -0700 ++ Using tf remapping as I should've been doing for awhile ++ r29277@att (orig r19911): eitanme | 2009-07-28 15:48:48 -0700 ++ The navigation stack can now handle goals that aren't in the global frame. However, these goals will be transformed to the global frame at the time of reception, so for achieving them accurately the global frame of move_base should really be set to match the goals. ++ r29299@att (orig r19933): stuglaser | 2009-07-28 17:08:10 -0700 ++ Created genaction.py script to create the various messages that an action needs ++ r29376@att (orig r20003): vijaypradeep | 2009-07-29 02:45:24 -0700 ++ ActionClient is running. MoveBase ActionServer seems to be crashing ++ r29409@att (orig r20033): vijaypradeep | 2009-07-29 11:57:54 -0700 ++ Fixing bug with adding status trackers ++ r29410@att (orig r20034): vijaypradeep | 2009-07-29 11:58:18 -0700 ++ Changing from Release to Debug ++ r29432@att (orig r20056): vijaypradeep | 2009-07-29 14:07:30 -0700 ++ No longer building goal_manager_test.cpp ++ r29472@att (orig r20090): vijaypradeep | 2009-07-29 17:04:14 -0700 ++ Lots of Client-Side doxygen ++ r29484@att (orig r20101): vijaypradeep | 2009-07-29 18:35:01 -0700 ++ Adding to mainpage.dox ++ r29487@att (orig r20104): eitanme | 2009-07-29 18:55:06 -0700 ++ Removing file to help resolve merge I hope ++ r29489@att (orig r20106): eitanme | 2009-07-29 19:00:07 -0700 ++ Removing another file to try to resolve the branch ++ r29492@att (orig r20108): eitanme | 2009-07-29 19:14:25 -0700 ++ Again removing a file to get the merge working ++ r29493@att (orig r20109): eitanme | 2009-07-29 19:34:45 -0700 ++ Removing yet another file on which ssl negotiation fails ++ r29500@att (orig r20116): eitanme | 2009-07-29 19:54:18 -0700 ++ Fixing bug in genaction ++* moving MapMetaData and OccGrid into nav_msgs `#1303 `_ ++* created nav_msgs and moved ParticleCloud there `#1300 `_ +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt b/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt +new file mode 100644 +index 00000000..b4751819 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt +@@ -0,0 +1,29 @@ ++cmake_minimum_required(VERSION 3.0.2) ++project(nav_msgs) ++ ++find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs actionlib_msgs) ++ ++add_message_files( ++ DIRECTORY msg ++ FILES ++ GridCells.msg ++ MapMetaData.msg ++ OccupancyGrid.msg ++ Odometry.msg ++ Path.msg) ++ ++add_service_files( ++ DIRECTORY srv ++ FILES ++ GetMap.srv ++ GetPlan.srv ++ SetMap.srv ++ LoadMap.srv) ++ ++add_action_files( ++ FILES ++ GetMap.action) ++ ++generate_messages(DEPENDENCIES geometry_msgs std_msgs actionlib_msgs) ++ ++catkin_package(CATKIN_DEPENDS geometry_msgs message_runtime std_msgs actionlib_msgs) +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action b/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action +new file mode 100644 +index 00000000..080e54fe +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action +@@ -0,0 +1,5 @@ ++# Get the map as a nav_msgs/OccupancyGrid ++--- ++nav_msgs/OccupancyGrid map ++--- ++# no feedback +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg +new file mode 100644 +index 00000000..2c928941 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg +@@ -0,0 +1,5 @@ ++#an array of cells in a 2D grid ++Header header ++float32 cell_width ++float32 cell_height ++geometry_msgs/Point[] cells +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg +new file mode 100644 +index 00000000..398fbdd0 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg +@@ -0,0 +1,13 @@ ++# This hold basic information about the characterists of the OccupancyGrid ++ ++# The time at which the map was loaded ++time map_load_time ++# The map resolution [m/cell] ++float32 resolution ++# Map width [cells] ++uint32 width ++# Map height [cells] ++uint32 height ++# The origin of the map [m, m, rad]. This is the real-world pose of the ++# cell (0,0) in the map. ++geometry_msgs/Pose origin +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg +new file mode 100644 +index 00000000..f2e79bda +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg +@@ -0,0 +1,11 @@ ++# This represents a 2-D grid map, in which each cell represents the probability of ++# occupancy. ++ ++Header header ++ ++#MetaData for the map ++MapMetaData info ++ ++# The map data, in row-major order, starting with (0,0). Occupancy ++# probabilities are in the range [0,100]. Unknown is -1. ++int8[] data +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg +new file mode 100644 +index 00000000..73578ed8 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg +@@ -0,0 +1,7 @@ ++# This represents an estimate of a position and velocity in free space. ++# The pose in this message should be specified in the coordinate frame given by header.frame_id. ++# The twist in this message should be specified in the coordinate frame given by the child_frame_id ++Header header ++string child_frame_id ++geometry_msgs/PoseWithCovariance pose ++geometry_msgs/TwistWithCovariance twist +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg +new file mode 100644 +index 00000000..979cb7d3 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg +@@ -0,0 +1,3 @@ ++#An array of poses that represents a Path for a robot to follow ++Header header ++geometry_msgs/PoseStamped[] poses +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/package.xml b/autoware.ai/src/autoware/messages/nav_msgs/package.xml +new file mode 100644 +index 00000000..f379061f +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/package.xml +@@ -0,0 +1,29 @@ ++ ++ nav_msgs ++ 1.13.1 ++ ++ nav_msgs defines the common messages used to interact with the ++ navigation stack. ++ ++ Michel Hidalgo ++ BSD ++ ++ http://wiki.ros.org/nav_msgs ++ Tully Foote ++ ++ catkin ++ ++ geometry_msgs ++ message_generation ++ std_msgs ++ actionlib_msgs ++ ++ geometry_msgs ++ message_runtime ++ std_msgs ++ actionlib_msgs ++ ++ ++ ++ ++ +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv +new file mode 100644 +index 00000000..6bd8e4a6 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv +@@ -0,0 +1,3 @@ ++# Get the map as a nav_msgs/OccupancyGrid ++--- ++nav_msgs/OccupancyGrid map +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv +new file mode 100644 +index 00000000..f5c23edb +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv +@@ -0,0 +1,13 @@ ++# Get a plan from the current position to the goal Pose ++ ++# The start pose for the plan ++geometry_msgs/PoseStamped start ++ ++# The final pose of the goal position ++geometry_msgs/PoseStamped goal ++ ++# If the goal is obstructed, how many meters the planner can ++# relax the constraint in x and y before failing. ++float32 tolerance ++--- ++nav_msgs/Path plan +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv +new file mode 100644 +index 00000000..3b9caaad +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv +@@ -0,0 +1,15 @@ ++# URL of map resource ++# Can be an absolute path to a file: file:///path/to/maps/floor1.yaml ++# Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml ++string map_url ++--- ++# Result code defintions ++uint8 RESULT_SUCCESS=0 ++uint8 RESULT_MAP_DOES_NOT_EXIST=1 ++uint8 RESULT_INVALID_MAP_DATA=2 ++uint8 RESULT_INVALID_MAP_METADATA=3 ++uint8 RESULT_UNDEFINED_FAILURE=255 ++ ++# Returned map is only valid if result equals RESULT_SUCCESS ++nav_msgs/OccupancyGrid map ++uint8 result +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv +new file mode 100644 +index 00000000..55d3c24f +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv +@@ -0,0 +1,6 @@ ++# Set a new map together with an initial pose ++nav_msgs/OccupancyGrid map ++geometry_msgs/PoseWithCovarianceStamped initial_pose ++--- ++bool success ++ +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt b/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt +index b2ae90cd..be86b1e7 100644 +--- a/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt +@@ -18,6 +18,9 @@ add_message_files( + TwistStamped.msg + VehicleCmd.msg + InsStat.msg ++ PoseTwistStamped.msg ++ LaneWithPoseTwist.msg ++ LaneArrayWithPoseTwist.msg + DetectedObjectArray.msg + ) + +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg +index dae5bd5d..5c47b6ac 100644 +--- a/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg +@@ -1,2 +1,3 @@ + uint64 instance +-autoware_msgs/DetectedObjectArray object_array +\ No newline at end of file ++autoware_msgs/DetectedObjectArray object_array ++uint64 obj_instance +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg +new file mode 100644 +index 00000000..a7c1df6f +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg +@@ -0,0 +1,5 @@ ++uint64 instance ++autoware_msgs/LaneArray lane_array ++geometry_msgs/PoseStamped pose ++geometry_msgs/TwistStamped twist ++uint64 obj_instance +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg +new file mode 100644 +index 00000000..05da6d2a +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg +@@ -0,0 +1,5 @@ ++uint64 instance ++autoware_msgs/Lane lane ++geometry_msgs/PoseStamped pose ++geometry_msgs/TwistStamped twist ++uint64 obj_instance +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg +new file mode 100644 +index 00000000..03d0fa20 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg +@@ -0,0 +1,3 @@ ++uint64 instance ++geometry_msgs/PoseStamped pose ++geometry_msgs/TwistStamped twist +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg +index b2cde46b..e4b90cd3 100644 +--- a/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg +@@ -1,2 +1,3 @@ + uint64 instance +-geometry_msgs/TwistStamped msg +\ No newline at end of file ++geometry_msgs/TwistStamped msg ++uint64 obj_instance +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch +new file mode 100644 +index 00000000..59c8f802 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch +@@ -0,0 +1,43 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch +new file mode 100644 +index 00000000..a14b621b +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch +@@ -0,0 +1,48 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch +new file mode 100644 +index 00000000..cd906d09 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch +new file mode 100644 +index 00000000..cf4d32cc +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch +@@ -0,0 +1,105 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch +new file mode 100644 +index 00000000..1fa79612 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch +@@ -0,0 +1,32 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..ee554f3a +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch +@@ -0,0 +1,44 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch +new file mode 100644 +index 00000000..59475396 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch +@@ -0,0 +1,51 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch +new file mode 100644 +index 00000000..66594cf2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch +new file mode 100644 +index 00000000..f25e94df +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch +@@ -0,0 +1,120 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch +new file mode 100644 +index 00000000..f66cef13 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch +@@ -0,0 +1,29 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/terminate.sh +new file mode 100644 +index 00000000..242e6f2c +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/terminate.sh +@@ -0,0 +1 @@ ++rosnode kill /cubetown +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch +new file mode 100644 +index 00000000..b410f94d +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch +@@ -0,0 +1,65 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch +new file mode 100644 +index 00000000..ab9971b3 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch +@@ -0,0 +1,60 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch +new file mode 100644 +index 00000000..6f2849ef +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch +@@ -0,0 +1,116 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch +new file mode 100644 +index 00000000..3653c630 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch +@@ -0,0 +1,32 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/terminate.sh +new file mode 100644 +index 00000000..230ba421 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/terminate.sh +@@ -0,0 +1 @@ ++rosnode kill /desktop_lane_keeping +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/test.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/test.launch +new file mode 100644 +index 00000000..1f08cd4a +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/test.launch +@@ -0,0 +1,26 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..a62287b4 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch +@@ -0,0 +1,58 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch +new file mode 100644 +index 00000000..49a153d4 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch +@@ -0,0 +1,57 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch +new file mode 100644 +index 00000000..66594cf2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch +new file mode 100644 +index 00000000..92401e07 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch +@@ -0,0 +1,105 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch +new file mode 100644 +index 00000000..41114441 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch +@@ -0,0 +1,38 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..be1dd9e2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch +@@ -0,0 +1,46 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch +new file mode 100644 +index 00000000..7d14bf71 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch +@@ -0,0 +1,20 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch +new file mode 100644 +index 00000000..66594cf2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch +new file mode 100644 +index 00000000..c15f91ce +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch +@@ -0,0 +1,106 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch +new file mode 100644 +index 00000000..7585a0a9 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch +@@ -0,0 +1,38 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..8d85ce76 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch +@@ -0,0 +1,57 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch +new file mode 100644 +index 00000000..af5aafc3 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch +@@ -0,0 +1,57 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch +new file mode 100644 +index 00000000..66594cf2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch +new file mode 100644 +index 00000000..899f293d +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch +@@ -0,0 +1,106 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch +new file mode 100644 +index 00000000..1b3fd1e0 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch +@@ -0,0 +1,38 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch +new file mode 100644 +index 00000000..c788f638 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch +@@ -0,0 +1,35 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch +new file mode 100644 +index 00000000..cf4d32cc +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch +@@ -0,0 +1,105 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch +new file mode 100755 +index 00000000..38c45a8a +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch +@@ -0,0 +1,50 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch +new file mode 100755 +index 00000000..c9bb8c2b +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch +@@ -0,0 +1,96 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch +new file mode 100755 +index 00000000..06c2022c +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch +@@ -0,0 +1,16 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch +new file mode 100755 +index 00000000..b1c32eae +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch +@@ -0,0 +1,13 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch +new file mode 100755 +index 00000000..105bff0b +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch +@@ -0,0 +1,225 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch +new file mode 100755 +index 00000000..1bd2b8cf +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch +@@ -0,0 +1,40 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch +new file mode 100755 +index 00000000..5d64aed7 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch +@@ -0,0 +1,89 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch +new file mode 100755 +index 00000000..6d826bb0 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch +@@ -0,0 +1,13 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch +new file mode 100755 +index 00000000..e11fc7dd +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch +@@ -0,0 +1,30 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/terminate.sh +new file mode 100755 +index 00000000..b2c331c0 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/terminate.sh +@@ -0,0 +1,56 @@ ++rosnode kill /lgsvl_triple_lidar_autorunner ++# rosnode kill /websocket_bridge ++# rosnode kill /lgsvl_traffic_signal_test ++# rosnode kill /image_republisher ++# rosnode kill /lidar_republisher_left ++# rosnode kill /lidar_republisher_right ++# rosnode kill /lidar_republisher_back ++# rosnode kill /camera_republisher ++# rosnode kill /gnss_republisher ++# rosnode kill /base_link_to_gnss ++# rosnode kill /base_link_to_velodyne ++# rosnode kill /vector_map_loader ++# rosnode kill /world_to_map ++# rosnode kill /map_to_mobility ++# rosnode kill /velodyne_to_velodyne_left ++# rosnode kill /velodyne_to_velodyne_right ++# rosnode kill /velodyne_to_velodyne_back ++# rosnode kill /pcd_clipper_left ++# rosnode kill /pcd_clipper_right ++# rosnode kill /pcd_clipper_back ++# rosnode kill /ray_ground_filter_left ++# rosnode kill /ray_ground_filter_right ++# rosnode kill /ray_ground_filter_back ++# rosnode kill /voxel_grid_filter ++# rosnode kill /gnss_localizer ++# rosnode kill /pose_relay ++# rosnode kill /vel_relay ++# rosnode kill /vision_darknet_detect ++# rosnode kill /yolo3_rects ++# rosnode kill /calibration_publisher_left ++# rosnode kill /calibration_publisher_right ++# rosnode kill /lidar_euclidean_cluster_detect_left ++# rosnode kill /lidar_euclidean_cluster_detect_right ++# rosnode kill /lidar_euclidean_cluster_detect_back ++# rosnode kill /imm_ukf_pda_left ++# rosnode kill /imm_ukf_pda_right ++# rosnode kill /imm_ukf_pda_back ++# rosnode kill /range_vision_fusion_left ++# rosnode kill /range_vision_fusion_right ++# rosnode kill /detection/fusion_tools/range_fusion_visualization_left ++# rosnode kill /detection/fusion_tools/range_fusion_visualization_right ++# rosnode kill /detection/lidar_detector/cluster_detect_visualization_back ++# rosnode kill /detection/lidar_detector/cluster_detect_visualization_left ++# rosnode kill /detection/lidar_detector/cluster_detect_visualization_right ++# rosnode kill /detection/object_tracker/ukf_track_visualization_back ++# rosnode kill /detection/object_tracker/ukf_track_visualization_left ++# rosnode kill /detection/object_tracker/ukf_track_visualization_right ++# rosnode kill op_global_planner ++# rosnode kill op_common_params ++# rosnode kill op_trajectory_generator ++# rosnode kill op_motion_predictor ++# rosnode kill predicted_objects_visualizer ++# rosnode kill op_trajectory_evaluator ++# rosnode kill op_behavior_selector ++# rosnode kill pure_pursuit ++# rosnode kill twist_filter +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/transformation_test/_transformation_test_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/transformation_test/_transformation_test_1_sensing.launch +new file mode 100644 +index 00000000..807e075f +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/transformation_test/_transformation_test_1_sensing.launch +@@ -0,0 +1,54 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/scripts/carla_test.launch b/rubis_ws/scripts/carla_test.launch +index 24a95b77..deb37689 100755 +--- a/rubis_ws/scripts/carla_test.launch ++++ b/rubis_ws/scripts/carla_test.launch +@@ -11,7 +11,15 @@ + + + +- ++ ++ ++ ++ ++ ++ ++ ++ ++ + + + +diff --git a/rubis_ws/src/camera_image/src/camera_image.cpp b/rubis_ws/src/camera_image/src/camera_image.cpp +index 0c747228..90d26635 100644 +--- a/rubis_ws/src/camera_image/src/camera_image.cpp ++++ b/rubis_ws/src/camera_image/src/camera_image.cpp +@@ -11,8 +11,6 @@ static const std::string OPENCV_WINDOW = "Raw Image Window"; + + static int camera_id = 0; + static int frequency = 0; +-static int task_scheduling_flag = 0; +-static int task_profiling_flag = 0; + static std::string task_response_time_filename; + // static int rate = 0; // Frequency replaces rate + static double task_minimum_inter_release_time = 0; +@@ -65,15 +63,13 @@ int main(int argc, char** argv){ + + pnh.param("/camera_image/camera_id", camera_id, 0); + pnh.param("/camera_image/frequency", frequency, 10); +- pnh.param("/camera_image/task_scheduling_flag", task_scheduling_flag, 0); +- pnh.param("/camera_image/task_profiling_flag", task_profiling_flag, 0); + pnh.param("/camera_image/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/camera_image.csv"); + // pnh.param("/camera_image/rate", rate, 10); // Frequency replaces rate + pnh.param("/camera_image/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000); + pnh.param("/camera_image/task_execution_time", task_execution_time, (double)100000000); + pnh.param("/camera_image/task_relative_deadline", task_relative_deadline, (double)100000000); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + ROS_INFO("camera_id : %d / frequency : %d",camera_id, frequency); + if(!frequency){ +@@ -104,12 +100,7 @@ void CameraImage::sendImage(){ + + while(nh_.ok()){ + +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ rubis::start_task_profiling(); + + cap >> frame; + if(!frame.empty()){ +@@ -118,17 +109,12 @@ void CameraImage::sendImage(){ + msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); + msg->header.frame_id="camera"; + camera_image_pub_.publish(msg); +- rubis::sched::is_task_ready_ = TASK_STATE_DONE; + } + // int ckey = cv::waitKey(1); + // if(ckey == 27)break; + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); ++ rubis::stop_task_profiling(0, 0); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } + loop_rate.sleep(); + } + } +diff --git a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch +index d7e1860a..7ea93364 100755 +--- a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch ++++ b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch +@@ -28,14 +28,14 @@ + remap carla lidar to autoware. + @todo: to reduce load, Autoware should directly use the Carla-topic. + --> +- ++ + + +- ++ + + ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive b/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive +index 3685e92d..69b53989 100755 +--- a/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive ++++ b/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive +@@ -34,9 +34,10 @@ def callback(data): + """ + global wheelbase + msg = AckermannDrive() +- msg.speed = data.twist_cmd.twist.linear.x ++ msg.speed = data.ctrl_cmd.linear_velocity + msg.steering_angle = convert_trans_rot_vel_to_steering_angle( + msg.speed, data.twist_cmd.twist.angular.z, wheelbase) ++ msg.acceleration = data.ctrl_cmd.linear_acceleration + pub.publish(msg) + + def twist_to_ackermanndrive(): +diff --git a/rubis_ws/src/gnss_converter/CMakeLists.txt b/rubis_ws/src/gnss_converter/CMakeLists.txt +new file mode 100755 +index 00000000..8c7a4f57 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/CMakeLists.txt +@@ -0,0 +1,97 @@ ++cmake_minimum_required(VERSION 3.0.2) ++project(gnss_converter) ++ ++if(NOT CMAKE_BUILD_TYPE) ++ set(CMAKE_BUILD_TYPE Release) ++endif() ++ ++set(CMAKE_CXX_FLAGS_RELEASE "-Ofast") ++ ++## Compile as C++11, supported in ROS Kinetic and newer ++# add_compile_options(-std=c++11) ++ ++find_package(catkin REQUIRED COMPONENTS ++ roscpp ++ std_msgs ++ inertiallabs_msgs ++ geometry_msgs ++ message_filters ++ tf ++) ++ ++find_package(Eigen3 REQUIRED) ++set(EIGEN_PACKAGE EIGEN3) ++if(NOT EIGEN3_FOUND) ++ find_package(cmake_modules REQUIRED) ++ find_package(Eigen REQUIRED) ++ set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) ++ set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) ++ set(EIGEN_PACKAGE Eigen) ++endif() ++ ++find_package(OpenCV REQUIRED) ++ ++catkin_package( ++# INCLUDE_DIRS include ++# LIBRARIES gnss_converter ++# CATKIN_DEPENDS roscpp std_msgs ++# DEPENDS system_lib ++) ++ ++include_directories( ++ include ++ ${catkin_INCLUDE_DIRS} ++ ${OpenCV_INCLUDE_DIRS} ++) ++ ++ ++add_executable(gnss_converter ++ include/gnss_converter/gnss_converter.h ++ include/gnss_converter/LLH2UTM.h ++ include/gnss_converter/quaternion_euler.h ++ src/gnss_converter.cpp ++ src/LLH2UTM.cpp ++ src/quaternion_euler.cpp ++) ++add_dependencies(gnss_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ++target_link_libraries(gnss_converter ++ ${catkin_LIBRARIES} ++ ${EIGEN3_LIBRARIES} ++ ${OpenCV_LIBS} ++) ++ ++add_executable(gnss_pose_pub ++ include/gnss_converter/LLH2UTM.h ++ include/gnss_converter/quaternion_euler.h ++ src/gnss_pose_pub.cpp ++ src/LLH2UTM.cpp ++ src/quaternion_euler.cpp ++) ++add_dependencies(gnss_pose_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ++target_link_libraries(gnss_pose_pub ++ ${catkin_LIBRARIES} ++ ${EIGEN3_LIBRARIES} ++ ${OpenCV_LIBS} ++) ++ ++install( ++ TARGETS ++ gnss_converter ++ gnss_pose_pub ++ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ++ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ++ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ++) ++ ++install(DIRECTORY include/ ++ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} ++ PATTERN ".svn" EXCLUDE ++) ++install(DIRECTORY launch/ ++ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ++ PATTERN ".svn" EXCLUDE ++) ++ ++install(DIRECTORY cfg/ ++ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg ++) +diff --git a/rubis_ws/src/gnss_converter/README.md b/rubis_ws/src/gnss_converter/README.md +new file mode 100644 +index 00000000..f3594dcb +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/README.md +@@ -0,0 +1,43 @@ ++# How to use gnss_converter (/gnss_pose publisher) ++### Case 1 : If you create a new map ++* Record /Inertial_Labs/gps_data, /Inertial_Labs/ins_data, /ndt_pose, /ndt_stat data while driving through the map (rosbag) ++```console ++~$ rosbag record /ndt_pose /ndt_stat /Inertial_Labs/gps_data /Inertial_Labs/ins_data -O [NAME] ++``` ++* Set /gnss_converter/calculate_tf of gnss_converter/cfg/gnsss_converter.yaml to true. ++```yaml ++/gnss_converter/calculate_tf : true ++``` ++* Set /gnss_converter/bag_file_path of gnss_converter/cfg/gnsss_converter.yaml to recorded rosbag file. ++```yaml ++/gnss_converter/bag_file_path : /home/sunhokim/Documents/RUBIS/data/220111_pose_gnss_138ground.bag # example ++``` ++* Run the following code ++```console ++~$ roslaunch gnss_converter gnss_converter.launch ++``` ++* Wait about 40 seconds and a image showing the route of the vehicle will appear on the screen. ++* You can select the four points used to calculate the transformation matrix through the following method. ++ - To change the image size, use track bar to select a value and press enter. ++ - If you want to choose a point, press the number and click the point in the image. (RBUTTONDOWN) ++ - If you want to end, press the ESC button. ++ - **WARNING** : When performing angle transformation, linear transformation was assumed. However, since the angle has a value between -180 degrees and +180 degrees, if there is a discontinuous interval between the four points, the matrix is not properly calculated. Therefore, when selecting four points, it is necessary to make sure that there is no section where the change in angle is discontinuous. (If the first value of the ori_tf matrix is close to -1.0, the calculation was performed well.) ++* After the previous process, the transformation matrix calculation result is displayed on the screen. ++* The calculation result is transferred to gnss_converter/cfg/gnss_converter.yaml. ++```yaml ++# ========= position tf matrix ========= ++/gnss_converter/pos_tf : [-1.372640, -1.384772, -27.949656, 13944.211735, 0.410968, -0.109160, -11.105078, -9490.840625, -0.318483, -0.086243, -6.272676, 453.012563, 0.000000, -0.000000, 0.000000, 1.000000] ++ ++# ========= orientation tf matrix ========= ++/gnss_converter/ori_tf : [-1.000448, 0.345540, 0.569044, 0.987319, 0.021691, -2.279683, -4.681593, -0.067717, 0.056971, 1.608331, -3.444783, -0.114943, 0.000000, -0.000000, 0.000000, 1.000000] ++``` ++* Move to Case 2 ++### Case 2 : If you've already done Case 1 ++* Set /gnss_converter/calculate_tf of gnss_converter/cfg/gnsss_converter.yaml to false. ++```yaml ++/gnss_converter/calculate_tf : false ++``` ++* Execute gnss_converter launch file. ++```console ++~$ roslaunch gnss_converter gnss_converter.launch ++``` +diff --git a/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml b/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml +new file mode 100644 +index 00000000..fa359036 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml +@@ -0,0 +1,13 @@ ++# true : calculate tf matrix ++# false : publish /gnss_pose topic ++/gnss_converter/calculate_tf : true ++ ++/gnss_converter/bag_file_path : /home/sunhokim/Documents/RUBIS/data/220111_pose_gnss_138ground.bag ++ ++# map : 220111_138ground ++ ++# ========= position tf matrix ========= ++/gnss_converter/pos_tf : [-1.372640, -1.384772, -27.949656, 13944.211735, 0.410968, -0.109160, -11.105078, -9490.840625, -0.318483, -0.086243, -6.272676, 453.012563, 0.000000, -0.000000, 0.000000, 1.000000] ++ ++# ========= orientation tf matrix ========= ++/gnss_converter/ori_tf : [-1.000448, 0.345540, 0.569044, 0.987319, 0.021691, -2.279683, -4.681593, -0.067717, 0.056971, 1.608331, -3.444783, -0.114943, 0.000000, -0.000000, 0.000000, 1.000000] +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h b/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h +new file mode 100644 +index 00000000..68728bc5 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h +@@ -0,0 +1,27 @@ ++#include ++#include ++#include ++ ++#include ++ ++/*************** parameters for LLH to UTM transformation ***************/ ++ ++#define WGS84_A 6378137.0 // major axis ++#define WGS84_B 6356752.31424518 // minor axis ++#define WGS84_F 0.0033528107 // ellipsoid flattening ++#define WGS84_E 0.0818191908 // first eccentricity ++#define WGS84_EP 0.0820944379 // second eccentricity ++ ++// UTM Parameters ++#define UTM_K0 0.9996 // scale factor ++#define UTM_FE 500000.0 // false easting ++#define UTM_FN_N 0.0 // false northing, northern hemisphere ++#define UTM_FN_S 10000000.0 // false northing, southern hemisphere ++#define UTM_E2 (WGS84_E*WGS84_E) // e^2 ++#define UTM_E4 (UTM_E2*UTM_E2) // e^4 ++#define UTM_E6 (UTM_E4*UTM_E2) // e^6 ++#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2 ++ ++/*************************************************************************/ ++ ++void LLH2UTM(double Lat, double Long, double H, geometry_msgs::PoseStamped& pose); +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h b/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h +new file mode 100644 +index 00000000..410e74ad +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h +@@ -0,0 +1,70 @@ ++#include ++#include ++#include ++#include ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#include ++ ++#include ++#include ++ ++#include ++#include ++#include ++#include ++ ++#include ++ ++#include ++#include ++#include ++#include ++ ++using namespace std; ++using namespace message_filters; ++using namespace Eigen; ++ ++#define ENTER_BUTTON 10 ++#define ESC_BUTTON 27 ++ ++struct gps_stat ++{ ++ std_msgs::Header header; ++ geometry_msgs::Point gps_pose; ++ geometry_msgs::Vector3 gps_ypr; ++ geometry_msgs::Point ndt_pose; ++ geometry_msgs::Vector3 ndt_ypr; ++ double ndt_score; ++}; ++ ++typedef sync_policies::ApproximateTime SyncPolicy_1; ++typedef sync_policies::ExactTime SyncPolicy_2; ++ ++static Matrix pos_tf_, ori_tf_; ++ ++static ros::Publisher gnss_pose_pub_; ++ ++static vector gps_backup_; ++static int ndt_pose_x_max_ = -9999999, ndt_pose_y_max_ = -9999999; ++static int ndt_pose_x_min_ = 9999999, ndt_pose_y_min_ = 9999999; ++static int scale_factor_ = 10; ++ ++static int points_idx_; ++static gps_stat selected_points_[4]; ++ ++void gps_ndt_data_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins, ++ const geometry_msgs::PoseStamped::ConstPtr &msg_ndt_pose); ++void pub_gnss_pose_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins); ++void track_bar_cb(int pos, void *userdata); ++void mouse_cb(int event, int x, int y, int flags, void *userdata); ++void points_select(); ++void calculate_tf_matrix(); +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h b/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h +new file mode 100644 +index 00000000..50b0c39d +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h +@@ -0,0 +1,7 @@ ++#include ++#include ++ ++#include ++ ++void ToEulerAngles(geometry_msgs::Quaternion q, double &yaw, double &pitch, double &roll); ++void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q); +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/launch/gnss_converter.launch b/rubis_ws/src/gnss_converter/launch/gnss_converter.launch +new file mode 100644 +index 00000000..658abfef +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/launch/gnss_converter.launch +@@ -0,0 +1,4 @@ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch b/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch +new file mode 100644 +index 00000000..8cdf1375 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch +@@ -0,0 +1,12 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/package.xml b/rubis_ws/src/gnss_converter/package.xml +new file mode 100644 +index 00000000..51cf2b79 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/package.xml +@@ -0,0 +1,23 @@ ++ ++ ++ gnss_converter ++ 0.0.0 ++ The gnss_converter package ++ ++ sunhokim ++ ++ Apache 2 ++ ++ catkin ++ roscpp ++ std_msgs ++ roscpp ++ std_msgs ++ roscpp ++ std_msgs ++ inertiallabs_msgs ++ message_filters ++ tf ++ eigen ++ libopencv-dev ++ +diff --git a/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp b/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp +new file mode 100644 +index 00000000..a39d4698 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp +@@ -0,0 +1,54 @@ ++#include ++ ++void LLH2UTM(double Lat, double Long, double H, geometry_msgs::PoseStamped& pose){ ++ double a = WGS84_A; ++ double eccSquared = UTM_E2; ++ double k0 = UTM_K0; ++ double LongOrigin; ++ double eccPrimeSquared; ++ double N, T, C, A, M; ++ // Make sure the longitude is between -180.00 .. 179.9 ++ // (JOQ: this is broken for Long < -180, do a real normalize) ++ double LongTemp = (Long+180)-int((Long+180)/360)*360-180; ++ double LatRad = angles::from_degrees(Lat); ++ double LongRad = angles::from_degrees(LongTemp); ++ double LongOriginRad; ++ pose.pose.position.z = H; ++ // Fix Zone number with Korea ++ int zone = 52; ++ char band = 'S'; ++ // +3 puts origin in middle of zone ++ LongOrigin = (zone - 1)*6 - 180 + 3; ++ LongOriginRad = angles::from_degrees(LongOrigin); ++ eccPrimeSquared = (eccSquared)/(1-eccSquared); ++ N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); ++ T = tan(LatRad)*tan(LatRad); ++ C = eccPrimeSquared*cos(LatRad)*cos(LatRad); ++ A = cos(LatRad)*(LongRad-LongOriginRad); ++ M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 ++ - 5*eccSquared*eccSquared*eccSquared/256) * LatRad ++ - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 ++ + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) ++ + (15*eccSquared*eccSquared/256 ++ + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) ++ - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); ++ pose.pose.position.y = (double) ++ (k0*N*(A+(1-T+C)*A*A*A/6 ++ + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) ++ + 500000.0); ++ pose.pose.position.x = (double) ++ (k0*(M+N*tan(LatRad) ++ *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 ++ + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); ++ ++ double TM[4][4] = ++ {{-0.821456, -0.593423, -0.006448, 3606301.475406}, ++ {-0.596954, 0.803991, -0.096993, 2231713.639404}, ++ {0.049875, 0.018177, -0.047063, -213252.081285}, ++ {0.000000, 0.000000, 0.000000, 1.000000}}; ++ ++ double input[4] = {pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, 1}; ++ pose.pose.position.x = TM[0][0]*input[0] + TM[0][1]*input[1] + TM[0][2]*input[2] + TM[0][3]*input[3]; ++ pose.pose.position.y = TM[1][0]*input[0] + TM[1][1]*input[1] + TM[1][2]*input[2] + TM[1][3]*input[3]; ++ pose.pose.position.z = TM[2][0]*input[0] + TM[2][1]*input[1] + TM[2][2]*input[2] + TM[2][3]*input[3]; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/src/gnss_converter.cpp b/rubis_ws/src/gnss_converter/src/gnss_converter.cpp +new file mode 100644 +index 00000000..964e49b6 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/src/gnss_converter.cpp +@@ -0,0 +1,363 @@ ++#include ++#include ++#include ++ ++void gps_ndt_data_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins, ++ const geometry_msgs::PoseStamped::ConstPtr &msg_ndt_pose) ++{ ++ ++ geometry_msgs::PoseStamped cur_pose; ++ ++ double ndt_yaw, ndt_pitch, ndt_roll; ++ ++ LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, cur_pose); ++ ++ ToEulerAngles(msg_ndt_pose->pose.orientation, ndt_yaw, ndt_pitch, ndt_roll); ++ ++ gps_stat tmp; ++ geometry_msgs::Vector3 vec_tmp; ++ ++ tmp.header = msg_gps->header; ++ ++ // gps position & orientation data ++ tmp.gps_pose = cur_pose.pose.position; ++ vec_tmp.x = ((msg_ins->YPR.x > 180) ? (msg_ins->YPR.x - 360) : (msg_ins->YPR.x)) / 180 * M_PI; ++ vec_tmp.y = ((msg_ins->YPR.y > 180) ? (msg_ins->YPR.y - 360) : (msg_ins->YPR.y)) / 180 * M_PI; ++ vec_tmp.z = ((msg_ins->YPR.z > 180) ? (msg_ins->YPR.z - 360) : (msg_ins->YPR.z)) / 180 * M_PI; ++ tmp.gps_ypr = vec_tmp; ++ ++ // ndt position & orientation data ++ tmp.ndt_pose = msg_ndt_pose->pose.position; ++ vec_tmp.x = ndt_yaw; ++ vec_tmp.y = ndt_pitch; ++ vec_tmp.z = ndt_roll; ++ tmp.ndt_ypr = vec_tmp; ++ ++ if (ndt_pose_x_max_ < msg_ndt_pose->pose.position.x) ++ ndt_pose_x_max_ = msg_ndt_pose->pose.position.x; ++ ++ if (ndt_pose_x_min_ > msg_ndt_pose->pose.position.x) ++ ndt_pose_x_min_ = msg_ndt_pose->pose.position.x; ++ ++ if (ndt_pose_y_max_ < msg_ndt_pose->pose.position.y) ++ ndt_pose_y_max_ = msg_ndt_pose->pose.position.y; ++ ++ if (ndt_pose_y_min_ > msg_ndt_pose->pose.position.y) ++ ndt_pose_y_min_ = msg_ndt_pose->pose.position.y; ++ ++ gps_backup_.push_back(tmp); ++} ++ ++void pub_gnss_pose_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins) ++{ ++ geometry_msgs::PoseStamped cur_pose; ++ ++ cur_pose.header = msg_ins->header; ++ cur_pose.header.frame_id = "/map"; ++ ++ Matrix gps; ++ Matrix ndt; ++ ++ /*================= position calculation =================*/ ++ LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, cur_pose); ++ ++ gps(0, 0) = cur_pose.pose.position.x; ++ gps(1, 0) = cur_pose.pose.position.y; ++ gps(2, 0) = cur_pose.pose.position.z; ++ gps(3, 0) = 1.0; ++ ++ ndt = pos_tf_ * gps; ++ ++ cur_pose.pose.position.x = ndt(0, 0); ++ cur_pose.pose.position.y = ndt(1, 0); ++ cur_pose.pose.position.z = ndt(2, 0); ++ /*=======================================================*/ ++ ++ /*=============== orientation calculation ===============*/ ++ gps(0, 0) = ((msg_ins->YPR.x > 180) ? (msg_ins->YPR.x - 360) : (msg_ins->YPR.x)) / 180 * M_PI; ++ gps(1, 0) = ((msg_ins->YPR.y > 180) ? (msg_ins->YPR.y - 360) : (msg_ins->YPR.y)) / 180 * M_PI; ++ gps(2, 0) = ((msg_ins->YPR.z > 180) ? (msg_ins->YPR.z - 360) : (msg_ins->YPR.z)) / 180 * M_PI; ++ gps(3, 0) = 1.0; ++ ++ ndt = ori_tf_ * gps; ++ ++ ToQuaternion(ndt(0, 0), ndt(1, 0), ndt(2, 0), cur_pose.pose.orientation); ++ ++ gnss_pose_pub_.publish(cur_pose); ++ /*=======================================================*/ ++} ++ ++void scale_image(int pos, void *userdata) ++{ ++ scale_factor_ = ((pos < 1) ? (1) : (pos)); ++} ++ ++void mouse_cb(int event, int x, int y, int flags, void *userdata) ++{ ++ if (event == cv::EVENT_RBUTTONDOWN) ++ { ++ double point_x = ndt_pose_x_min_ + x / scale_factor_; ++ double point_y = ndt_pose_y_min_ + y / scale_factor_; ++ double min_dist = 9999; ++ int idx; ++ ++ for (int i = 0; i < gps_backup_.size(); i++) ++ { ++ if (pow((point_x - gps_backup_[i].ndt_pose.x), 2) + pow((point_y - gps_backup_[i].ndt_pose.y), 2) < pow(min_dist, 2)) ++ { ++ idx = i; ++ min_dist = pow((point_x - gps_backup_[i].ndt_pose.x), 2) + pow((point_y - gps_backup_[i].ndt_pose.y), 2); ++ } ++ } ++ ++ selected_points_[points_idx_] = gps_backup_[idx]; ++ std::cout << "**************** Point INFO ****************" << std::endl; ++ ++ std::cout << "GPS Pose" << std::endl; ++ std::cout << " x : " << selected_points_[points_idx_].gps_pose.x << std::endl; ++ std::cout << " y : " << selected_points_[points_idx_].gps_pose.y << std::endl; ++ std::cout << " z : " << selected_points_[points_idx_].gps_pose.z << std::endl; ++ ++ std::cout << "GPS YPR" << std::endl; ++ std::cout << " yaw : " << selected_points_[points_idx_].gps_ypr.x / M_PI * 180 << std::endl; ++ std::cout << " pitch : " << selected_points_[points_idx_].gps_ypr.y / M_PI * 180 << std::endl; ++ std::cout << " roll : " << selected_points_[points_idx_].gps_ypr.z / M_PI * 180 << std::endl; ++ ++ std::cout << "NDT Pose" << std::endl; ++ std::cout << " x : " << selected_points_[points_idx_].ndt_pose.x << std::endl; ++ std::cout << " y : " << selected_points_[points_idx_].ndt_pose.y << std::endl; ++ std::cout << " z : " << selected_points_[points_idx_].ndt_pose.z << std::endl; ++ ++ std::cout << "NDT YPR" << std::endl; ++ std::cout << " yaw : " << selected_points_[points_idx_].ndt_ypr.x / M_PI * 180 << std::endl; ++ std::cout << " pitch : " << selected_points_[points_idx_].ndt_ypr.y / M_PI * 180 << std::endl; ++ std::cout << " roll : " << selected_points_[points_idx_].ndt_ypr.z / M_PI * 180 << std::endl; ++ ++ std::cout << "NDT score" << selected_points_[points_idx_].ndt_score << std::endl; ++ } ++} ++ ++void points_select() ++{ ++ cv::Mat orig_img = cv::Mat((ndt_pose_y_max_ - ndt_pose_y_min_ + 1), (ndt_pose_x_max_ - ndt_pose_x_min_ + 1), CV_8UC3, cv::Scalar(255, 255, 255)); ++ cv::Mat display_img; ++ ++ for (int i = 0; i < gps_backup_.size(); i++) ++ { ++ cv::circle(orig_img, cv::Point((gps_backup_[i].ndt_pose.x - ndt_pose_x_min_), (gps_backup_[i].ndt_pose.y - ndt_pose_y_min_)), ++ 1, cv::Scalar(255, 255, 0), 1, cv::LINE_AA); ++ } ++ ++ orig_img.copyTo(display_img); ++ ++ cv::namedWindow("Display Image"); ++ cv::createTrackbar("Scale", "Display Image", &scale_factor_, 100, scale_image, NULL); ++ ++ cv::setMouseCallback("Display Image", mouse_cb, NULL); ++ ++ std::cout << "**************** How To Use ****************" << std::endl; ++ std::cout << "If you want to end, press the ESC button." << std::endl; ++ std::cout << "If you want to choose a point, press the number and click the point in the image." << std::endl; ++ std::cout << "To change the image size, use track bar to select a value and press enter." << std::endl; ++ std::cout << "********************************************" << std::endl; ++ ++ int keycode; ++ while (true) ++ { ++ display_img = cv::Mat(orig_img.rows * scale_factor_, orig_img.cols * scale_factor_, CV_8UC3, cv::Scalar(255, 255, 255)); ++ for (int i = 0; i < gps_backup_.size(); i++) ++ { ++ cv::circle(display_img, ++ cv::Point(scale_factor_ * (gps_backup_[i].ndt_pose.x - ndt_pose_x_min_), scale_factor_ * (gps_backup_[i].ndt_pose.y - ndt_pose_y_min_)), ++ 1, cv::Scalar(255, 255, 0), 1, cv::LINE_AA); ++ } ++ ++ cv::imshow("Display Image", display_img); ++ keycode = cv::waitKey(); ++ ++ if (keycode == ESC_BUTTON) ++ break; ++ ++ switch (keycode) ++ { ++ case '1': ++ points_idx_ = 0; ++ std::cout << "Point Number : 1" << std::endl; ++ break; ++ case '2': ++ points_idx_ = 1; ++ std::cout << "Point Number : 2" << std::endl; ++ break; ++ case '3': ++ points_idx_ = 2; ++ std::cout << "Point Number : 3" << std::endl; ++ break; ++ case '4': ++ points_idx_ = 3; ++ std::cout << "Point Number : 4" << std::endl; ++ break; ++ case ENTER_BUTTON: ++ break; ++ default: ++ std::cout << "Unknown keyboard input" << std::endl; ++ break; ++ } ++ } ++} ++ ++void calculate_tf_matrix() ++{ ++ Matrix gps_pos, ndt_pos, gps_ypr, ndt_ypr; ++ ++ for (int i = 0; i < 4; i++) ++ { ++ gps_pos(0, i) = selected_points_[i].gps_pose.x; ++ gps_pos(1, i) = selected_points_[i].gps_pose.y; ++ gps_pos(2, i) = selected_points_[i].gps_pose.z; ++ gps_pos(3, i) = 1.0; ++ ++ ndt_pos(0, i) = selected_points_[i].ndt_pose.x; ++ ndt_pos(1, i) = selected_points_[i].ndt_pose.y; ++ ndt_pos(2, i) = selected_points_[i].ndt_pose.z; ++ ndt_pos(3, i) = 1.0; ++ ++ gps_ypr(0, i) = selected_points_[i].gps_ypr.x; ++ gps_ypr(1, i) = selected_points_[i].gps_ypr.y; ++ gps_ypr(2, i) = selected_points_[i].gps_ypr.z; ++ gps_ypr(3, i) = 1.0; ++ ++ ndt_ypr(0, i) = selected_points_[i].ndt_ypr.x; ++ ndt_ypr(1, i) = selected_points_[i].ndt_ypr.y; ++ ndt_ypr(2, i) = selected_points_[i].ndt_ypr.z; ++ ndt_ypr(3, i) = 1.0; ++ } ++ ++ pos_tf_ = ndt_pos * gps_pos.inverse(); ++ ori_tf_ = ndt_ypr * gps_ypr.inverse(); ++ ++ printf("========== position tf matrix ==========\n"); ++ printf("[%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf]\n", pos_tf_(0, 0), pos_tf_(0, 1), pos_tf_(0, 2), pos_tf_(0, 3), pos_tf_(1, 0), pos_tf_(1, 1), pos_tf_(1, 2), pos_tf_(1, 3), pos_tf_(2, 0), pos_tf_(2, 1), pos_tf_(2, 2), pos_tf_(2, 3), pos_tf_(3, 0), pos_tf_(3, 1), pos_tf_(3, 2), pos_tf_(3, 3)); ++ printf("======== orientation tf matrix =========\n"); ++ printf("[%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf]\n", ori_tf_(0, 0), ori_tf_(0, 1), ori_tf_(0, 2), ori_tf_(0, 3), ori_tf_(1, 0), ori_tf_(1, 1), ori_tf_(1, 2), ori_tf_(1, 3), ori_tf_(2, 0), ori_tf_(2, 1), ori_tf_(2, 2), ori_tf_(2, 3), ori_tf_(3, 0), ori_tf_(3, 1), ori_tf_(3, 2), ori_tf_(3, 3)); ++} ++ ++int main(int argc, char *argv[]) ++{ ++ ros::init(argc, argv, "gnss_converter"); ++ ++ ros::NodeHandle nh; ++ ++ bool calculate_tf; ++ ++ gnss_pose_pub_ = nh.advertise("/gnss_pose", 10); ++ ++ message_filters::Subscriber gps_sub(nh, "/Inertial_Labs/gps_data", 10); ++ message_filters::Subscriber ndt_pose_sub(nh, "/ndt_pose", 10); ++ message_filters::Subscriber ins_sub(nh, "/Inertial_Labs/ins_data", 10); ++ ++ Synchronizer sync_1(SyncPolicy_1(100), gps_sub, ins_sub, ndt_pose_sub); ++ Synchronizer sync_2(SyncPolicy_2(10), gps_sub, ins_sub); ++ ++ ros::param::get("/gnss_converter/calculate_tf", calculate_tf); ++ ++ if (calculate_tf) ++ { ++ sync_1.registerCallback(boost::bind(&gps_ndt_data_cb, _1, _2, _3)); ++ ++ pid_t pid; ++ if ((pid = fork()) < 0) ++ ROS_ERROR("Cannot create child!"); ++ ++ if (pid == 0) ++ { ++ std::string file_path; ++ char file_path_cstr[150]; ++ ++ ros::param::get("/gnss_converter/bag_file_path", file_path); ++ ++ int str_len = file_path.length(); ++ if (str_len >= 150) ++ { ++ ROS_ERROR("File path is too long!!"); ++ exit(0); ++ } ++ ++ strcpy(file_path_cstr, file_path.c_str()); ++ ++ const char *file = "/opt/ros/melodic/bin/rosbag"; ++ char *exe_argv[] = {"/opt/ros/melodic/bin/rosbag", ++ "play", ++ "-r", ++ "5", ++ file_path_cstr, ++ NULL}; ++ ++ if (execvp(file, exe_argv) < 0) ++ { ++ ROS_ERROR("Cannot load bag file!!"); ++ } ++ } ++ else ++ { ++ int wstatus; ++ while (waitpid(pid, &wstatus, WNOHANG) == 0) ++ { ++ ros::spinOnce(); ++ } ++ ROS_WARN("Finish loading data from rosbag file!!"); ++ points_select(); ++ calculate_tf_matrix(); ++ } ++ } ++ ++ else ++ { ++ sync_2.registerCallback(boost::bind(&pub_gnss_pose_cb, _1, _2)); ++ ++ vector tf_tmp; ++ ++ /*================= pos_tf_ matrix =================*/ ++ ros::param::get("/gnss_converter/pos_tf", tf_tmp); ++ pos_tf_(0, 0) = tf_tmp[0]; ++ pos_tf_(0, 1) = tf_tmp[1]; ++ pos_tf_(0, 2) = tf_tmp[2]; ++ pos_tf_(0, 3) = tf_tmp[3]; ++ pos_tf_(1, 0) = tf_tmp[4]; ++ pos_tf_(1, 1) = tf_tmp[5]; ++ pos_tf_(1, 2) = tf_tmp[6]; ++ pos_tf_(1, 3) = tf_tmp[7]; ++ pos_tf_(2, 0) = tf_tmp[8]; ++ pos_tf_(2, 1) = tf_tmp[9]; ++ pos_tf_(2, 2) = tf_tmp[10]; ++ pos_tf_(2, 3) = tf_tmp[11]; ++ pos_tf_(3, 0) = tf_tmp[12]; ++ pos_tf_(3, 1) = tf_tmp[13]; ++ pos_tf_(3, 2) = tf_tmp[14]; ++ pos_tf_(3, 3) = tf_tmp[15]; ++ /*=================================================*/ ++ ++ /*================= ori_tf_ matrix =================*/ ++ ros::param::get("/gnss_converter/ori_tf", tf_tmp); ++ ori_tf_(0, 0) = tf_tmp[0]; ++ ori_tf_(0, 1) = tf_tmp[1]; ++ ori_tf_(0, 2) = tf_tmp[2]; ++ ori_tf_(0, 3) = tf_tmp[3]; ++ ori_tf_(1, 0) = tf_tmp[4]; ++ ori_tf_(1, 1) = tf_tmp[5]; ++ ori_tf_(1, 2) = tf_tmp[6]; ++ ori_tf_(1, 3) = tf_tmp[7]; ++ ori_tf_(2, 0) = tf_tmp[8]; ++ ori_tf_(2, 1) = tf_tmp[9]; ++ ori_tf_(2, 2) = tf_tmp[10]; ++ ori_tf_(2, 3) = tf_tmp[11]; ++ ori_tf_(3, 0) = tf_tmp[12]; ++ ori_tf_(3, 1) = tf_tmp[13]; ++ ori_tf_(3, 2) = tf_tmp[14]; ++ ori_tf_(3, 3) = tf_tmp[15]; ++ /*================================================*/ ++ ++ ros::spin(); ++ } ++ ++ return 0; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp b/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp +new file mode 100644 +index 00000000..ad248edd +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp +@@ -0,0 +1,91 @@ ++#include ++#include ++ ++#include ++#include ++#include ++ ++#include ++#include ++#include ++#include ++ ++#include ++#include ++#include ++#include ++ ++#define M_PI 3.14159265358979323846 ++ ++using namespace std; ++using namespace message_filters; ++ ++typedef sync_policies::ExactTime SyncPolicy_; ++ ++static ros::Publisher gnss_pose_pub_; ++static double x_offset_, y_offset_, yaw_offset_; ++static double roll_, pitch_, yaw_; ++static geometry_msgs::PoseStamped gnss_pose_; ++ ++void gnss_pose_pub_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins){ ++ gnss_pose_.header = msg_gps->header; ++ gnss_pose_.header.frame_id = "/map"; ++ ++ LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, gnss_pose_); ++ ++ gnss_pose_.pose.position.x = gnss_pose_.pose.position.x - x_offset_; ++ gnss_pose_.pose.position.y = gnss_pose_.pose.position.y - y_offset_; ++ ++ roll_ = msg_ins->YPR.z; ++ pitch_ = msg_ins->YPR.y; ++ ++ yaw_ = msg_ins->YPR.x; ++ yaw_ *= -1; ++ yaw_ -= yaw_offset_; ++ if(yaw_ > 180.0) yaw_ -= 360.0; ++ if(yaw_ < -180.0) yaw_ += 360.0; ++ ++ roll_ = roll_ * M_PI/180.0; ++ pitch_ = pitch_ * M_PI/180.0; ++ yaw_ = yaw_ * M_PI/180.0; ++} ++ ++int main(int argc, char *argv[]){ ++ ros::init(argc, argv, "gnss_pose_pub"); ++ ++ ros::NodeHandle nh; ++ ++ nh.param("/gnss_pose_pub/x_offset", x_offset_, 0.0); ++ nh.param("/gnss_pose_pub/y_offset", y_offset_, 0.0); ++ nh.param("/ins_twist_generator/yaw_offset", yaw_offset_, 0.0); ++ ++ gnss_pose_pub_ = nh.advertise("/ndt_pose", 2); ++ ++ message_filters::Subscriber gps_sub(nh, "/Inertial_Labs/gps_data", 2); ++ message_filters::Subscriber ins_sub(nh, "/Inertial_Labs/ins_data", 2); ++ ++ Synchronizer sync(SyncPolicy_(2), gps_sub, ins_sub); ++ ++ sync.registerCallback(boost::bind(&gnss_pose_pub_cb, _1, _2)); ++ ++ tf::TransformBroadcaster br; ++ ros::Rate r(10); ++ while(nh.ok()){ ++ ros::spinOnce(); ++ ++ ToQuaternion(yaw_, pitch_, roll_, gnss_pose_.pose.orientation); ++ ++ tf::Quaternion q; ++ q.setRPY(roll_, pitch_, yaw_); ++ ++ tf::StampedTransform transform; ++ transform.setOrigin(tf::Vector3(gnss_pose_.pose.position.x, gnss_pose_.pose.position.y, 0.0)); ++ transform.setRotation(q); ++ br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/map", "/base_link")); ++ ++ gnss_pose_pub_.publish(gnss_pose_); ++ ++ ++ r.sleep(); ++ } ++} +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp b/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp +new file mode 100644 +index 00000000..158c979b +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp +@@ -0,0 +1,32 @@ ++#include ++ ++void ToEulerAngles(geometry_msgs::Quaternion q, double &yaw, double &pitch, double &roll){ ++ double sinr_cosp = 2 * (q.w * q.x + q.y * q.z); ++ double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y); ++ roll = std::atan2(sinr_cosp, cosr_cosp); ++ ++ double sinp = 2 * (q.w * q.y - q.z * q.x); ++ if (std::abs(sinp) >= 1) ++ pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range ++ else ++ pitch = std::asin(sinp); ++ ++ double siny_cosp = 2 * (q.w * q.z + q.x * q.y); ++ double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); ++ yaw = std::atan2(siny_cosp, cosy_cosp); ++} ++ ++void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q) ++{ ++ double cy = cos(yaw * 0.5); ++ double sy = sin(yaw * 0.5); ++ double cp = cos(pitch * 0.5); ++ double sp = sin(pitch * 0.5); ++ double cr = cos(roll * 0.5); ++ double sr = sin(roll * 0.5); ++ ++ q.w = cr * cp * cy + sr * sp * sy; ++ q.x = sr * cp * cy - cr * sp * sy; ++ q.y = cr * sp * cy + sr * cp * sy; ++ q.z = cr * cp * sy - sr * sp * cy; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/image_common/image_transport/src/republish.cpp b/rubis_ws/src/image_common/image_transport/src/republish.cpp +index 589e351a..2a8140d6 100644 +--- a/rubis_ws/src/image_common/image_transport/src/republish.cpp ++++ b/rubis_ws/src/image_common/image_transport/src/republish.cpp +@@ -54,23 +54,19 @@ int main(int argc, char** argv) + + // scheduling + ros::NodeHandle pnh("~"); +- int task_scheduling_flag = 0; +- int task_profiling_flag = 0; + std::string task_response_time_filename; + int rate = 0; + double task_minimum_inter_release_time = 0; + double task_execution_time = 0; + double task_relative_deadline = 0; + +- pnh.param("/republish/task_scheduling_flag", task_scheduling_flag, 0); +- pnh.param("/republish/task_profiling_flag", task_profiling_flag, 0); + pnh.param("/republish/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/republish.csv"); + pnh.param("/republish/rate", rate, 10); + pnh.param("/republish/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000); + pnh.param("/republish/task_execution_time", task_execution_time, (double)100000000); + pnh.param("/republish/task_relative_deadline", task_relative_deadline, (double)100000000); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + if (argc < 3) { + // Use all available transports for output +@@ -81,34 +77,17 @@ int main(int argc, char** argv) + PublishMemFn pub_mem_fn = &image_transport::Publisher::publish; + sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, &pub, _1), ros::VoidPtr(), in_transport); + +- rubis::sched::task_state_ = TASK_STATE_READY; ++ ros::Rate r(rate); ++ // Executing task ++ while(ros::ok()){ ++ rubis::start_task_profiling(); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ ros::spinOnce(); ++ + +- ros::spinOnce(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ rubis::stop_task_profiling(0, 0); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ r.sleep(); + } + } + else { +@@ -128,32 +107,14 @@ int main(int argc, char** argv) + PublishMemFn pub_mem_fn = &Plugin::publish; + sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, pub.get(), _1), pub, in_transport); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- rubis::sched::task_state_ = TASK_STATE_DONE; +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ ros::Rate r(rate); ++ // Executing task ++ while(ros::ok()){ ++ rubis::start_task_profiling(); ++ ros::spinOnce(); ++ ++ rubis::stop_task_profiling(0, 0); ++ r.sleep(); + } + } + +diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt b/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt +index 66aa1a13..bf1bf2a9 100644 +--- a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt ++++ b/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt +@@ -15,7 +15,6 @@ inertiallabs_msgs + + include_directories(${catkin_INCLUDE_DIRS}) + +-find_package(OpenCV) + + set(DEPRECATION_FLAG "-Wsizeof-array-argument") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra ${DEPRECATION_FLAG}") +@@ -26,7 +25,6 @@ catkin_package( + DEPENDS system_lib + ) + +-include_directories(${Opencv_INCLUDE_DIRS}) + include_directories(${system_lib_INCLUDE_DIRS}) + + include_directories(../inertiallabs_sdk/) +diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml +index c49e3f72..40654dad 100755 +--- a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml ++++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml +@@ -15,4 +15,4 @@ carla_ackermann_control: + # at more or less constant speed. + # If the absolute value of the ackermann drive target acceleration exceeds this value, + # directly the input acceleration is controlled +- min_accel: 1.0 ++ min_accel: 0.005 +diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup +new file mode 100755 +index 00000000..c49e3f72 +--- /dev/null ++++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup +@@ -0,0 +1,18 @@ ++carla_ackermann_control: ++ ros__parameters: ++ # override the default values of the pid speed controller ++ # (only relevant for ackermann control mode) ++ speed_Kp: 0.05 # min: 0, max: 1. ++ speed_Ki: 0.00 # min: 0, max: 1. ++ speed_Kd: 0.50 # min: 0, max: 10. ++ # override the default values of the pid acceleration controller ++ # (only relevant for ackermann control mode) ++ accel_Kp: 0.05 # min: 0, max: 10. ++ accel_Ki: 0.00 # min: 0, max: 10. ++ accel_Kd: 0.05 # min: 0, max: 10. ++ # set the minimum acceleration in (m/s^2) ++ # This border value is used to enable the speed controller which is used to control driving ++ # at more or less constant speed. ++ # If the absolute value of the ackermann drive target acceleration exceeds this value, ++ # directly the input acceleration is controlled ++ min_accel: 1.0 +diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py b/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py +index 944f70fd..c8d29c00 100755 +--- a/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py ++++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py +@@ -442,6 +442,7 @@ class CarlaAckermannControl(CompatibleNode): + else: + if self.info.status.speed_control_activation_count > 0: + self.info.status.speed_control_activation_count -= 1 ++ + # set the auto_mode of the controller accordingly + self.speed_controller.auto_mode = self.info.status.speed_control_activation_count >= 5 + +diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json +index 9aef8118..d31f2c9f 100755 +--- a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json ++++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json +@@ -55,13 +55,14 @@ + "type": "sensor.lidar.ray_cast", + "id": "lidar", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, +- "range": 50, +- "channels": 32, ++ "range": 100, ++ "channels": 16, + "points_per_second": 320000, + "upper_fov": 2.0, + "lower_fov": -26.8, + "rotation_frequency": 20, +- "noise_stddev": 0.0 ++ "noise_stddev": 0.0, ++ "sensor_tick": 0.1 + }, + { + "type": "sensor.other.gnss", +diff --git a/rubis_ws/src/rubis_autorunner/CMakeLists.txt b/rubis_ws/src/rubis_autorunner/CMakeLists.txt +index 4932f0b1..be91e751 100644 +--- a/rubis_ws/src/rubis_autorunner/CMakeLists.txt ++++ b/rubis_ws/src/rubis_autorunner/CMakeLists.txt +@@ -48,6 +48,18 @@ add_executable(minicar_lane_keeping + src/minicar_lane_keeping/minicar_lane_keeping.cpp + ) + ++add_executable(carla_lkas_autorunner ++ include/carla_autorunner/carla_autorunner.h ++ src/carla_autorunner/carla_lkas_autorunner_node.cpp ++ src/carla_autorunner/carla_lkas_autorunner.cpp ++) ++ ++add_executable(carla_full_autorunner ++ include/carla_autorunner/carla_autorunner.h ++ src/carla_autorunner/carla_full_autorunner_node.cpp ++ src/carla_autorunner/carla_full_autorunner.cpp ++) ++ + add_executable(cubetown_lkas_autorunner + include/cubetown_autorunner/cubetown_autorunner.h + src/cubetown_autorunner/cubetown_lkas_autorunner_node.cpp +@@ -102,6 +114,19 @@ target_link_libraries(cubetown_full_autorunner + ros_autorunner_lib + ) + ++add_dependencies(carla_lkas_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ++target_link_libraries(carla_lkas_autorunner ++ ${catkin_LIBRARIES} ++ ros_autorunner_lib ++) ++ ++add_dependencies(carla_full_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ++target_link_libraries(carla_full_autorunner ++ ${catkin_LIBRARIES} ++ ros_autorunner_lib ++) ++ ++ + add_dependencies(tutorial_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(tutorial_autorunner + ${catkin_LIBRARIES} +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml +new file mode 100644 +index 00000000..feb87474 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml +@@ -0,0 +1,183 @@ ++# Unit of rate: hz ++# Unit of scheduling params: ns ++ ++# 1s : 1_000_000_000 ++# 1ms : 1_000_000 ++# 1us : 1_000 ++# 1ns : 1 ++ ++#rate = hz ++# other time unut = ns ++ ++# Lane Keeping ++lidar_republisher: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 99 ++ exec_time: 1_000_000 ++ deadline: 2_000_000 ++ period: 2_000_000 ++ task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" ++ ++voxel_grid_filter: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 99 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" ++ ++ndt_matching: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" ++ ++ use_kalman_filter: False ++ tf_x: 1.0510799 ++ tf_y: 0 ++ tf_z: 1.96 ++ tf_roll: 0 ++ tf_pitch: 0 ++ tf_yaw: 0 ++ localizer: "velodyne" ++ ++pure_pursuit: ++ rate: 30 #30 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" ++ ++ dynamic_params_flag: False ++ dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" ++ ++twist_filter: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" ++ ++# Detection ++ray_ground_filter_center: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" ++ ++lidar_euclidean_cluster_detect: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" ++ ++ network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" ++ pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" ++ ++# Planning ++op_global_planner: ++ rate: 25 #25 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" ++ ++ multilap_flag: 1 ++ ++op_common_params: ++ rollOutDensity: 4 ++ rollOutsNumber: 2 ++ maxVelocity: 10.0 ++ maxAcceleration: 10.0 ++ maxDeceleration: -10.0 ++ ++op_trajectory_generator: ++ rate: 100 #100 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" ++ ++op_trajectory_evaluator: ++ rate: 100 #100 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" ++ ++ weightPriority: 1 ++ weightTransition: 0.5 ++ weightLong: 1 ++ weightLat: 1 ++ ImageWidth: 1920 ++ ImageHeight: 1080 ++ SprintDecisionTime: 9999999.0 ++ ++op_behavior_selector: ++ rate: 100 #100 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" ++ ++ distanceToPedestrianThreshold: 15.0 ++ sprintSpeed: 10.0 ++ obstacleWaitingTimeinIntersection: 2.0 ++ turnThreshold: 30.0 ++ ++op_motion_predictor: ++ rate: 25 #25 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" ++ ++# Independent ++twist_gate: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" ++ ++ zero_flag: 0 ## Publish target velocity as 0 +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml +new file mode 100644 +index 00000000..83c242dd +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml +@@ -0,0 +1,14 @@ ++total_step_num: 5 ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh" ++# step_# : ++# [0] pacakge name ++# [1] target name( node name or launch script name ) ++# [2] state that create constraint topic or not ( "true" or "false" ) ++# [3] state that check constraint topic or not ( "true" or "false" ) ++# [4] target type( RUN or LAUNCH ) ++ ++step_1: ["rubis_autorunner", "_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"] ++step_2: ["rubis_autorunner", "_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"] ++step_3: ["rubis_autorunner", "_carla_autorunner_3_detection.launch", "true", "true", "LAUNCH"] ++step_4: ["rubis_autorunner", "_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"] ++step_5: ["rubis_autorunner", "_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"] +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml +new file mode 100644 +index 00000000..6fed9fc7 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml +@@ -0,0 +1,13 @@ ++total_step_num: 4 ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh" ++# step_# : ++# [0] pacakge name ++# [1] target name( node name or launch script name ) ++# [2] state that create constraint topic or not ( "true" or "false" ) ++# [3] state that check constraint topic or not ( "true" or "false" ) ++# [4] target type( RUN or LAUNCH ) ++ ++step_1: ["rubis_autorunner", "_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"] ++step_2: ["rubis_autorunner", "_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"] ++step_3: ["rubis_autorunner", "_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"] ++step_4: ["rubis_autorunner", "_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"] +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla/lane_info.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/lane_info.yaml +similarity index 100% +rename from rubis_ws/src/rubis_autorunner/cfg/carla/lane_info.yaml +rename to rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/lane_info.yaml +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml +similarity index 80% +rename from rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml +rename to rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml +index 1680abe6..a7f3c4f3 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml +@@ -12,7 +12,7 @@ + # Sensing + ray_ground_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -22,7 +22,7 @@ ray_ground_filter: + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -33,17 +33,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 100 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + logger_brake_dist: + rate: 10 +@@ -55,37 +50,27 @@ calibration_publisher: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 100 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -94,7 +79,7 @@ imm_ukf_pda_track: + + range_vision_fusion: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -104,7 +89,7 @@ range_vision_fusion: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -122,7 +107,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -131,7 +116,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100000000 +@@ -154,7 +139,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -167,7 +152,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 40000000 +@@ -177,7 +162,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 33333333 +@@ -188,7 +173,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -197,7 +182,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml.backup b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml.backup +index 4e759a90..40581461 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml.backup ++++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml.backup +@@ -1,5 +1,5 @@ + total_step_num: 5 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml +index dacb51ae..a9545721 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml +@@ -1,30 +1,46 @@ + # Unit of rate: hz +-# Unit of task information: nano seconds ++# Unit of scheduling params: ns + +-# 1s : 1000000000 +-# 1ms : 1000000 +-# 1us : 1000 ++# 1s : 1_000_000_000 ++# 1ms : 1_000_000 ++# 1us : 1_000 + # 1ns : 1 + + #rate = hz + # other time unut = ns + +-# Localization +-voxel_grid_filter: +- instance_mode: 1 ++# Lane Keeping ++lidar_republisher: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 99 ++ exec_time: 1_000_000 ++ deadline: 2_000_000 ++ period: 2_000_000 ++ task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" ++voxel_grid_filter: + rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 1_600_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 99 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + + ndt_matching: +- instance_mode: 1 +- use_kalman_filter: True +- ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" ++ ++ use_kalman_filter: False + tf_x: 1.0510799 + tf_y: 0 + tf_z: 1.96 +@@ -33,129 +49,93 @@ ndt_matching: + tf_yaw: 0 + localizer: "velodyne" + +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 80_000_000 +- task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" +- +-modular_ndt_matching_CENTER: +- instance_mode: 1 +- use_kalman_filter: True ++pure_pursuit: ++ rate: 30 #30 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + +- tf_x: 1.0510799 +- tf_y: 0 +- tf_z: 1.96 +- tf_roll: 0 +- tf_pitch: 0 +- tf_yaw: 0 +- localizer: "velodyne" ++ dynamic_params_flag: False ++ dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" ++twist_filter: + rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 80_000_000 +- task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + + # Detection + ray_ground_filter_center: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 100_000_000 +- task_relative_deadline: 100_000_000 +- ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + + lidar_euclidean_cluster_detect: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 +- task_minimum_inter_release_time: 200_000_000 +- task_execution_time: 50_000_000 +- task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" +- +-vision_darknet_detect: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 75_000_000 +- task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" ++ + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + +-imm_ukf_pda_track: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" +- rate: 10 +- task_minimum_inter_release_time: 200_000_000 +- task_execution_time: 100_000_000 +- task_relative_deadline: 200_000_000 +- + # Planning + op_global_planner: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 45_000_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" ++ + multilap_flag: 1 + + op_common_params: +- rollOutDensity: 2 +- rollOutsNumber: 4 ++ rollOutDensity: 4 ++ rollOutsNumber: 2 + maxVelocity: 10.0 + maxAcceleration: 10.0 + maxDeceleration: -10.0 + + op_trajectory_generator: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 2_900_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + + op_trajectory_evaluator: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 3_600_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" ++ + weightPriority: 1 + weightTransition: 0.5 + weightLong: 10 +@@ -165,77 +145,39 @@ op_trajectory_evaluator: + SprintDecisionTime: 9999999.0 + + op_behavior_selector: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 20_800_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" ++ + distanceToPedestrianThreshold: 15.0 + sprintSpeed: 10.0 + obstacleWaitingTimeinIntersection: 2.0 + turnThreshold: 30.0 + + op_motion_predictor: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 3_800_000 +- task_relative_deadline: 100_000_000 +- +-# Control +-pure_pursuit: +- instance_mode: 1 +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" +- rate: 30 #30 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 3_000_000 +- task_relative_deadline: 100_000_000 +- dynamic_params_flag: False +- dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" +- +-twist_filter: +- instance_mode: 1 +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 2_000_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + ++# Independent + twist_gate: +- instance_mode: 1 +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 2_000_000 +- task_relative_deadline: 100_000_000 +- zero_flag: 0 ## Publish target velocity as 0 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + +-# Others +-lidar_republisher: +- instance_mode: 1 +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 70_000_000 +- task_relative_deadline: 100_000_000 +- +-republish: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 35_000_000 +- task_relative_deadline: 100_000_000 +\ No newline at end of file ++ zero_flag: 0 ## Publish target velocity as 0 +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup +deleted file mode 100644 +index f8e2f9cf..00000000 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup ++++ /dev/null +@@ -1,238 +0,0 @@ +-# Unit of rate: hz +-# Unit of task information: nano seconds +- +-# 1s : 1000000000 +-# 1ms : 1000000 +-# 1us : 1000 +-# 1ns : 1 +- +-#rate = hz +-# other time unut = ns +- +-# Localization +-voxel_grid_filter: +- task_scheduling_flag: 1 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-ndt_matching: +- tf_x: 1.0510799 +- tf_y: 0 +- tf_z: 1.96 +- tf_roll: 0 +- tf_pitch: 0 +- tf_yaw: 0 +- localizer: "velodyne" +- +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 70000000 +- task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" +- +-# Detection +-compare_map_filter: +- distance_threshold: 0.4 +- min_clipping_height: -1.7 +- max_clipping_height: 0.5 +- +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 70000000 +- task_relative_deadline: 100000000 +- +- +-lidar_euclidean_cluster_detect: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" +- +-vision_darknet_detect: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" +- network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg" +- pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights" +- +-imm_ukf_pda_track: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-range_vision_fusion: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-# Planning +-op_global_planner: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" +- rate: 25 #25 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- multilap_flag: 1 +- +-op_common_params: +- rollOutDensity: 4 +- rollOutsNumber: 0 +- maxVelocity: 10.0 +- maxAcceleration: 10.0 +- maxDeceleration: -10.0 +- +-op_trajectory_generator: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" +- rate: 100 #100 +- task_minimum_inter_release_time: 10000000 +- task_execution_time: 10000000 +- task_relative_deadline: 10000000 +- +-op_trajectory_evaluator: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" +- rate: 100 #100 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- weightPriority: 0 +- weightTransition: 5 +- weightLong: 4 +- weightLat: 4 +- ImageWidth: 1920 +- ImageHeight: 1080 +- SprintDecisionTime: 9999999.0 +- +-op_behavior_selector: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" +- rate: 100 #100 +- task_minimum_inter_release_time: 10000000 +- task_execution_time: 10000000 +- task_relative_deadline: 10000000 +- distanceToPedestrianThreshold: 15.0 +- sprintSpeed: 10.0 +- obstacleWaitingTimeinIntersection: 2.0 +- turnThreshold: 30.0 +- +-op_motion_predictor: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" +- rate: 25 #25 +- task_minimum_inter_release_time: 40000000 +- task_execution_time: 40000000 +- task_relative_deadline: 40000000 +- +-# Control +-pure_pursuit: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" +- rate: 30 #30 +- task_minimum_inter_release_time: 33333333 +- task_execution_time: 33333333 +- task_relative_deadline: 33333333 +- dynamic_params_flag: False +- dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" +- +-twist_filter: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-twist_gate: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- zero_flag: 0 ## Publish target velocity as 0 +- +-# Others +-lidar_republisher: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-vel_relay: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-vel_relay: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-republish: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml +index 4e759a90..40581461 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 5 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml +index c70a8e39..6873e2f3 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 4 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml +index 3548041c..f142af8a 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml +@@ -1,5 +1,5 @@ + total_step_num: 4 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml +index 29cc8960..0817c790 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml +@@ -12,7 +12,7 @@ + # Sensing + ray_ground_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -22,7 +22,7 @@ ray_ground_filter: + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -33,17 +33,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + calibration_publisher: +@@ -51,37 +46,27 @@ calibration_publisher: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -90,7 +75,7 @@ imm_ukf_pda_track: + + range_vision_fusion: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -100,7 +85,7 @@ range_vision_fusion: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -118,7 +103,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -127,7 +112,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100000000 +@@ -143,7 +128,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -156,7 +141,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 40000000 +@@ -166,7 +151,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 33333333 +@@ -177,7 +162,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -186,7 +171,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml +index 446ed3b2..0eaf97db 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml +@@ -19,7 +19,7 @@ voxel_grid_filter: + instance_mode: 1 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -30,20 +30,14 @@ modular_ndt_matching_FR: + instance_mode: 1 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + ndt_matching: +- instance_mode: 1 + use_kalman_filter: False + + tf_x: 3.3 +@@ -55,24 +49,17 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" +- +- + + # Detection + ray_ground_filter_center: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -82,37 +69,27 @@ ray_ground_filter_center: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 + task_execution_time: 50_000_000 + task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 75_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 +@@ -122,7 +99,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 10 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -139,7 +116,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 10 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -148,7 +125,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 10 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -166,7 +143,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 10 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -179,7 +156,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 10 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -188,9 +165,8 @@ op_motion_predictor: + + # Control + pure_pursuit: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 10 #30 + task_minimum_inter_release_time: 100_000_000 +@@ -200,9 +176,8 @@ pure_pursuit: + dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + + twist_filter: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -210,9 +185,8 @@ twist_filter: + task_relative_deadline: 100_000_000 + + twist_gate: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -222,9 +196,8 @@ twist_gate: + + # Others + lidar_republisher: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -233,7 +206,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -241,9 +214,8 @@ vel_relay: + task_relative_deadline: 100_000_000 + + pose_relay: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -252,7 +224,7 @@ pose_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml +index cb127f78..f979d738 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml +@@ -18,7 +18,7 @@ voxel_grid_filter: + instance_mode: 1 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -29,20 +29,14 @@ modular_ndt_matching_FR: + instance_mode: 1 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + ndt_matching: +- instance_mode: 1 + use_kalman_filter: False + + tf_x: 3.3 +@@ -54,24 +48,17 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" +- +- + + # Detection + ray_ground_filter_center: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -81,37 +68,27 @@ ray_ground_filter_center: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 40 + task_minimum_inter_release_time: 200_000_000 + task_execution_time: 50_000_000 + task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 75_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 40 + task_minimum_inter_release_time: 200_000_000 +@@ -121,7 +98,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 40 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -138,7 +115,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 40 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -147,7 +124,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 40 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -165,7 +142,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 40 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -178,7 +155,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 40 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -187,9 +164,8 @@ op_motion_predictor: + + # Control + pure_pursuit: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 40 #30 + task_minimum_inter_release_time: 100_000_000 +@@ -199,9 +175,8 @@ pure_pursuit: + dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + + twist_filter: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -209,9 +184,8 @@ twist_filter: + task_relative_deadline: 100_000_000 + + twist_gate: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -221,9 +195,8 @@ twist_gate: + + # Others + lidar_republisher: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -232,7 +205,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -240,9 +213,8 @@ vel_relay: + task_relative_deadline: 100_000_000 + + pose_relay: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -251,7 +223,7 @@ pose_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml +index 717968c5..e0e4d6ab 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 9 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml +index 1f7f7e4e..b1249827 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml +@@ -1,5 +1,5 @@ + total_step_num: 4 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/minicar_lane_keeping/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/minicar_lane_keeping/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml +index 17751521..b2066de6 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml +@@ -15,7 +15,7 @@ camera_image: + frequency: 10 + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + # rate: 10 # Frequency replaces rate + task_minimum_inter_release_time: 100000000 +@@ -24,7 +24,7 @@ camera_image: + + ray_ground_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -34,7 +34,7 @@ ray_ground_filter: + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -45,22 +45,17 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -78,7 +73,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -87,7 +82,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100000000 +@@ -103,7 +98,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -117,7 +112,7 @@ op_behavior_selector: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 33333333 +@@ -128,7 +123,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -137,7 +132,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -148,7 +143,7 @@ twist_gate: + # Others + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -157,7 +152,7 @@ vel_relay: + + pose_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml +index 7c9a8b8d..b4235314 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 5 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml +index 59cfa745..ccd8802d 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml +@@ -14,7 +14,7 @@ voxel_grid_filter: + instance_mode: 0 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -33,22 +33,17 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + ray_ground_filter_center: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -58,37 +53,27 @@ ray_ground_filter_center: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 + task_execution_time: 50_000_000 + task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 75_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 +@@ -98,7 +83,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -115,7 +100,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -124,7 +109,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -140,7 +125,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -153,7 +138,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -162,9 +147,8 @@ op_motion_predictor: + + # Control + pure_pursuit: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 100_000_000 +@@ -174,9 +158,8 @@ pure_pursuit: + dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + + twist_filter: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -184,9 +167,8 @@ twist_filter: + task_relative_deadline: 100_000_000 + + twist_gate: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -196,9 +178,8 @@ twist_gate: + + # Others + lidar_republisher: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -207,7 +188,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -215,9 +196,8 @@ vel_relay: + task_relative_deadline: 100_000_000 + + pose_relay: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -226,7 +206,7 @@ pose_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml +index 7bbaa3d4..89161057 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 5 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/tutorial/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/tutorial/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml +index 5ff124da..39f6cf02 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml +@@ -12,7 +12,7 @@ + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -29,17 +29,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 1 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + compare_map_filter: +@@ -48,7 +43,7 @@ compare_map_filter: + max_clipping_height: 0.5 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 +@@ -58,37 +53,27 @@ compare_map_filter: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 + task_execution_time: 50_000_000 + task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 1 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 75_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 1 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 +@@ -98,7 +83,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -115,7 +100,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -124,7 +109,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -140,7 +125,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -153,7 +138,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -163,7 +148,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 100_000_000 +@@ -174,7 +159,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -183,7 +168,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -194,7 +179,7 @@ twist_gate: + # Others + lidar_republisher: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -203,7 +188,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -212,7 +197,7 @@ vel_relay: + + pose_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -221,7 +206,7 @@ pose_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +diff --git a/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h b/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h +new file mode 100644 +index 00000000..66bf4692 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h +@@ -0,0 +1,32 @@ ++#include ++#include ++ ++// Include subscribe message type ++#include ++#include ++#include ++#include ++#include ++#include ++#define SLEEP_PERIOD 1 ++ ++class CarlaAutorunner : public AutorunnerBase{ ++private: ++ ros::NodeHandle nh_; ++ ROSAutorunner ros_autorunner_; ++private: ++ virtual void register_subscribers(); ++private: ++ void points_raw_cb(const sensor_msgs::PointCloud2& msg); ++ void ndt_pose_cb(const geometry_msgs::PoseStamped& msg); ++ void detection_cb(const autoware_msgs::DetectedObjectArray& msg); ++ void behavior_state_cb(const visualization_msgs::MarkerArray& msg); ++ ++public: ++ Sub_v sub_v_; ++ ros::Publisher initial_pose_pub_; ++public: ++ CarlaAutorunner() {} ++ CarlaAutorunner(ros::NodeHandle nh) : nh_(nh){} ++ virtual void Run(); ++}; +diff --git a/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h b/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h +index a7a7086a..84f62d3a 100644 +--- a/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h ++++ b/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h +@@ -6,6 +6,7 @@ + #include + #include + #include ++#include + #include + #define SLEEP_PERIOD 1 + +@@ -17,7 +18,7 @@ private: + virtual void register_subscribers(); + private: + void points_raw_cb(const sensor_msgs::PointCloud2& msg); +- void ndt_stat_cb(const autoware_msgs::NDTStat& msg); ++ void ndt_pose_cb(const geometry_msgs::PoseStamped& msg); + void detection_cb(const autoware_msgs::DetectedObjectArray& msg); + void behavior_state_cb(const visualization_msgs::MarkerArray& msg); + +diff --git a/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch +new file mode 100644 +index 00000000..a91009bc +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch +@@ -0,0 +1,4 @@ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch +new file mode 100644 +index 00000000..41f53b5f +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch +@@ -0,0 +1,4 @@ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..50ce0be4 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch +@@ -0,0 +1,46 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch +new file mode 100644 +index 00000000..9f6dca50 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch +@@ -0,0 +1,54 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch +new file mode 100644 +index 00000000..df533eb6 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch +@@ -0,0 +1,42 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch +new file mode 100644 +index 00000000..8bb60870 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch +@@ -0,0 +1,64 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch +new file mode 100644 +index 00000000..4f5d25e6 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch +@@ -0,0 +1,118 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch +new file mode 100644 +index 00000000..59144340 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch +@@ -0,0 +1,32 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch +index 072b973a..fdf35425 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch +@@ -22,17 +22,21 @@ + + + +- ++ + + + + + + +- + +- ++ + + +- ++ ++ ++ ++ ++ + ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch +index cf62f8fb..8afcb8a1 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch +@@ -36,11 +36,11 @@ + pitch: $(arg init_pitch), + yaw: $(arg init_yaw), + use_predict_pose: 1, +- error_threshold: 0.01, +- resolution: 3.0, +- step_size: 0.3, ++ error_threshold: 0.05, ++ resolution: 1.0, ++ step_size: 0.5, + trans_epsilon: 0.01, +- max_iterations: 10} ++ max_iterations: 2} + '"/> + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch +index a49a410e..819a2793 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch +@@ -82,11 +82,18 @@ + + + ++ ++ + + + + + ++ ++ ++ ++ ++ + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch +index 68b2046e..6fbbc3a2 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch +@@ -15,7 +15,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch +index a8f5c037..b399ee97 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch +@@ -21,24 +21,19 @@ + + + +- +- + + + +- + + + + + +- + + + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch +index fd7ce414..05ed4386 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch +@@ -15,7 +15,6 @@ + + + +- + + + +@@ -33,7 +32,6 @@ + + + +- + + + +@@ -78,7 +76,6 @@ + + + +- + + + +@@ -123,7 +120,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch +index cab08d89..546fc8d7 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch +@@ -36,22 +36,18 @@ + + + +- + + + + + + +- +- +- ++ + + + + + +- + + + +@@ -76,7 +72,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch +index e78be3d2..44a69ccc 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch +@@ -15,7 +15,6 @@ + + + +- + + + +@@ -23,7 +22,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch +index 2a317ad2..c71066ba 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch +@@ -14,7 +14,6 @@ + + + +- + + + +@@ -32,7 +31,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp +new file mode 100644 +index 00000000..e5f44ead +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp +@@ -0,0 +1,108 @@ ++#include "carla_autorunner/carla_autorunner.h" ++ ++void CarlaAutorunner::Run(){ ++ register_subscribers(); // Register subscribers that shoud check can go next or not ++ ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner ++ ros::Rate rate(1); // Rate can be changed ++ while(ros::ok()){ ++ if(!ros_autorunner_.Run()) break; // Run Autorunner ++ ros::spinOnce(); ++ rate.sleep(); ++ } ++} ++ ++void CarlaAutorunner::register_subscribers(){ ++ int total_step_num = nh_.param("/total_step_num", -1); ++ if(total_step_num < 0){ ++ std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); ++} ++ ++ void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ ++ if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ ++ ROS_WARN("[STEP 1] Map and Sensors are prepared"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; ++ } ++ } ++ ++ void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ ++ static int failure_cnt = 0, success_cnt = 0; ++ failure_cnt++; ++ ++ static const double pos_x = 314.072479248; ++ static const double pos_y = 129.654495239; ++ static const double pos_z = 0.044597864151; ++ ++ static const double ori_x = 0.0; ++ static const double ori_y = 0.0; ++ static const double ori_z = 0.70715665039; ++ static const double ori_w = 0.70705690407; ++ ++ ++ if(failure_cnt > 10){ ++ std::cout<<"# Refresh inital pose"<= pos_x - 1.5 && ++ msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 && ++ msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 && ++ msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 && ++ msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 && ++ msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 && ++ !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ ++ success_cnt++; ++ if(success_cnt < 3) return; ++ ROS_WARN("[STEP 2] Localization is success"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; ++ } ++ else{ ++ success_cnt = 0; ++ } ++ } ++ ++void CarlaAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){ ++ if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){ ++ ROS_WARN("[STEP 3] All detection modules are excuted"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; ++ } ++} ++ ++ ++ void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ ++ std::string state = msg.markers.front().text; ++ if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ ++ ROS_WARN("[STEP 4] Global & local planning success"); ++ ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true; ++ } ++} ++ ++ ++ +diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp +new file mode 100644 +index 00000000..caac332f +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp +@@ -0,0 +1,11 @@ ++#include ++ ++int main(int argc, char* argv[]){ ++ ros::init(argc, argv, "carla_autorunner"); ++ ros::NodeHandle nh; ++ ++ CarlaAutorunner carla_autorunner(nh); ++ carla_autorunner.Run(); ++ ++ return 0; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp +new file mode 100644 +index 00000000..df8f65dd +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp +@@ -0,0 +1,99 @@ ++#include "carla_autorunner/carla_autorunner.h" ++ ++void CarlaAutorunner::Run(){ ++ register_subscribers(); // Register subscribers that shoud check can go next or not ++ ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner ++ ros::Rate rate(1); // Rate can be changed ++ while(ros::ok()){ ++ if(!ros_autorunner_.Run()) break; // Run Autorunner ++ ros::spinOnce(); ++ rate.sleep(); ++ } ++} ++ ++void CarlaAutorunner::register_subscribers(){ ++ int total_step_num = nh_.param("/total_step_num", -1); ++ if(total_step_num < 0){ ++ std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); ++} ++ ++ void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ ++ if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ ++ ROS_WARN("[STEP 1] Map and Sensors are prepared"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; ++ } ++ } ++ ++ void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ ++ static int failure_cnt = 0, success_cnt = 0; ++ failure_cnt++; ++ ++ static const double pos_x = 314.072479248; ++ static const double pos_y = 129.654495239; ++ static const double pos_z = 0.044597864151; ++ ++ static const double ori_x = 0.0; ++ static const double ori_y = 0.0; ++ static const double ori_z = 0.70715665039; ++ static const double ori_w = 0.70705690407; ++ ++ ++ if(failure_cnt > 10){ ++ std::cout<<"# Refresh inital pose"<= pos_x - 1.5 && ++ msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 && ++ msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 && ++ msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 && ++ msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 && ++ msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 && ++ !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ ++ success_cnt++; ++ if(success_cnt < 3) return; ++ ROS_WARN("[STEP 2] Localization is success"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; ++ } ++ else{ ++ success_cnt = 0; ++ } ++ } ++ ++ ++ void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ ++ std::string state = msg.markers.front().text; ++ if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ ++ ROS_WARN("[STEP 3] Global & local planning success"); ++ ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; ++ } ++} ++ ++ ++ +diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp +new file mode 100644 +index 00000000..caac332f +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp +@@ -0,0 +1,11 @@ ++#include ++ ++int main(int argc, char* argv[]){ ++ ros::init(argc, argv, "carla_autorunner"); ++ ros::NodeHandle nh; ++ ++ CarlaAutorunner carla_autorunner(nh); ++ carla_autorunner.Run(); ++ ++ return 0; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp +index 62a816ed..2dc84b89 100644 +--- a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp ++++ b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp +@@ -21,7 +21,7 @@ void CubetownAutorunner::register_subscribers(){ + + // Set the check function(subscriber) + sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CubetownAutorunner::points_raw_cb, this); +- sub_v_[STEP(2)] = nh_.subscribe("/ndt_stat", 1, &CubetownAutorunner::ndt_stat_cb, this); ++ sub_v_[STEP(2)] = nh_.subscribe("/ndt_pose", 1, &CubetownAutorunner::ndt_pose_cb, this); + sub_v_[STEP(3)] = nh_.subscribe("/detection/lidar_detector/objects_center", 1, &CubetownAutorunner::detection_cb, this); + sub_v_[STEP(4)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this); + +@@ -36,27 +36,37 @@ void CubetownAutorunner::register_subscribers(){ + } + } + +- void CubetownAutorunner::ndt_stat_cb(const autoware_msgs::NDTStat& msg){ +- static int cnt = 0; +- cnt++; +- if(cnt > 200){ +- geometry_msgs::PoseWithCovariance msg; +- msg.pose.position.x = 56.3796081543; +- msg.pose.position.y = -0.0106279850006; +- msg.pose.position.z = 0.465716004372; +- msg.pose.orientation.x = -0.00171861096474; +- msg.pose.orientation.y = -0.00120572400155; +- msg.pose.orientation.z = 0.707457658123; +- msg.pose.orientation.w = 0.706752612; +- initial_pose_pub_.publish(msg); +- cnt = 0; ++ void CubetownAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ ++ static int failure_cnt = 0, success_cnt = 0; ++ failure_cnt++; ++ ++ if(failure_cnt > 10){ ++ std::cout<<"# Refresh inital pose"<= 55.0 && ++ msg.pose.position.y >= -0.20 && msg.pose.position.y <= 0.00 && ++ !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ ++ success_cnt++; ++ if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } ++ else{ ++ success_cnt = 0; ++ } + + } + +diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp +index fecbc2c3..422d1062 100644 +--- a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp ++++ b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp +@@ -21,7 +21,7 @@ void CubetownAutorunner::register_subscribers(){ + + // Set the check function(subscriber) + sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CubetownAutorunner::points_raw_cb, this); +- sub_v_[STEP(2)] = nh_.subscribe("/ndt_stat", 1, &CubetownAutorunner::ndt_stat_cb, this); ++ sub_v_[STEP(2)] = nh_.subscribe("/ndt_pose", 1, &CubetownAutorunner::ndt_pose_cb, this); + sub_v_[STEP(3)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this); + + initial_pose_pub_ = nh_.advertise< geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1); +@@ -35,29 +35,41 @@ void CubetownAutorunner::register_subscribers(){ + } + } + +- void CubetownAutorunner::ndt_stat_cb(const autoware_msgs::NDTStat& msg){ +- static int cnt = 0; +- cnt++; +- if(cnt > 200){ +- geometry_msgs::PoseWithCovariance msg; +- msg.pose.position.x = 56.3796081543; +- msg.pose.position.y = -0.0106279850006; +- msg.pose.position.z = 0.465716004372; +- msg.pose.orientation.x = -0.00171861096474; +- msg.pose.orientation.y = -0.00120572400155; +- msg.pose.orientation.z = 0.707457658123; +- msg.pose.orientation.w = 0.706752612; +- initial_pose_pub_.publish(msg); +- cnt = 0; ++ void CubetownAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ ++ static int failure_cnt = 0, success_cnt = 0; ++ failure_cnt++; ++ ++ if(failure_cnt > 10){ ++ std::cout<<"# Refresh inital pose"<= 55.0 && ++ msg.pose.position.y >= -0.20 && msg.pose.position.y <= 0.00 && ++ !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ ++ success_cnt++; ++ if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } ++ else{ ++ success_cnt = 0; ++ } ++ + } + ++ + void CubetownAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ + std::string state = msg.markers.front().text; + if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ +diff --git a/rubis_ws/src/rubis_pkg/include/gnss_localizer.h b/rubis_ws/src/rubis_pkg/include/gnss_localizer.h +index 317a5987..1edf9048 100644 +--- a/rubis_ws/src/rubis_pkg/include/gnss_localizer.h ++++ b/rubis_ws/src/rubis_pkg/include/gnss_localizer.h +@@ -19,6 +19,7 @@ + #include + #include + #include ++#include + + namespace gnss_localizer + { +diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch b/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch +index 488d479d..3f5a0762 100644 +--- a/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch ++++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch +@@ -2,11 +2,8 @@ + + + +- +- ++ + + +- +- + + +diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch +index 84c50fc6..3f5a0762 100644 +--- a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch ++++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch +@@ -2,10 +2,8 @@ + + + +- +- ++ + + +- + + +diff --git a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch +index 28ae3347..031c4ecc 100644 +--- a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch ++++ b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch +@@ -5,11 +5,8 @@ + + + +- + +- +- +- ++ + + + +diff --git a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch +index 7abb5b98..031c4ecc 100644 +--- a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch ++++ b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch +@@ -5,11 +5,8 @@ + + + +- + +- +- +- ++ + + + +diff --git a/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp b/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp +index 7f094c5d..a7c7a342 100644 +--- a/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp ++++ b/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp +@@ -70,17 +70,39 @@ void Nmea2TFPoseNode::initForROS() + } + + // setup subscriber +- sub1_ = nh_.subscribe("nmea_sentence", 100, &Nmea2TFPoseNode::callbackFromNmeaSentence, this); +- sub2_ = nh_.subscribe("imu_raw", 100, &Nmea2TFPoseNode::callbackFromIMU, this); ++ sub1_ = nh_.subscribe("nmea_sentence", 1, &Nmea2TFPoseNode::callbackFromNmeaSentence, this); ++ sub2_ = nh_.subscribe("imu_raw", 1, &Nmea2TFPoseNode::callbackFromIMU, this); + + // setup publisher + if(enable_offset_){ +- pub1_ = nh_.advertise("gnss_offset_pose", 10); +- pub2_ = nh_.advertise("gnss_transformed_pose", 10); ++ pub1_ = nh_.advertise("gnss_offset_pose", 1); ++ pub2_ = nh_.advertise("gnss_transformed_pose", 1); + } + else +- pub1_ = nh_.advertise("gnss_pose", 10); +- vel_pub_ = nh_.advertise("gnss_vel", 10); ++ pub1_ = nh_.advertise("gnss_pose", 1); ++ vel_pub_ = nh_.advertise("gnss_vel", 1); ++ ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); ++ std::string task_response_time_filename; ++ nh_.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/gnss_localizer.csv"); ++ ++ int rate; ++ nh_.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ nh_.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ nh_.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ nh_.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ nh_.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ nh_.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); + } + + void Nmea2TFPoseNode::run() +diff --git a/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp b/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp +index 8d24cf4e..2a4da217 100644 +--- a/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp ++++ b/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp +@@ -9,6 +9,8 @@ static ros::Publisher pub, pub_rubis; + int is_topic_ready = 1; + + void points_cb(const sensor_msgs::PointCloud2ConstPtr& msg){ ++ rubis::start_task_profiling(); ++ + sensor_msgs::PointCloud2 msg_with_intensity = *msg; + + msg_with_intensity.fields.at(3).datatype = 7; +@@ -16,15 +18,31 @@ void points_cb(const sensor_msgs::PointCloud2ConstPtr& msg){ + + pub.publish(msg_with_intensity); + +- if(rubis::instance_mode_){ +- rubis_msgs::PointCloud2 rubis_msg_with_intensity; +- rubis_msg_with_intensity.instance = rubis::instance_; +- rubis_msg_with_intensity.msg = msg_with_intensity; +- pub_rubis.publish(rubis_msg_with_intensity); +- } ++ rubis_msgs::PointCloud2 rubis_msg_with_intensity; ++ rubis_msg_with_intensity.instance = rubis::instance_; ++ rubis_msg_with_intensity.msg = msg_with_intensity; ++ pub_rubis.publish(rubis_msg_with_intensity); + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ rubis::instance_ = rubis::instance_+1; ++ rubis::obj_instance_ = rubis::obj_instance_+1; ++} ++ ++std::string exec(const char* cmd) { ++ char buffer[128]; ++ std::string result = ""; ++ FILE* pipe = popen(cmd, "r"); ++ if (!pipe) throw std::runtime_error("popen() failed!"); ++ try { ++ while (fgets(buffer, sizeof(buffer), pipe) != NULL) { ++ result += buffer; ++ } ++ } catch (...) { ++ pclose(pipe); ++ throw; ++ } ++ pclose(pipe); ++ return result; + } + + int main(int argc, char** argv){ +@@ -39,72 +57,36 @@ int main(int argc, char** argv){ + std::string output_topic_name = node_name + "/output_topic"; + std::string rubis_output_topic; + +- nh.param(node_name+"/instance_mode", rubis::instance_mode_, 0); + nh.param(input_topic_name, input_topic, "/points_raw_origin"); + nh.param(output_topic_name, output_topic, "/points_raw"); + + sub = nh.subscribe(input_topic, 1, points_cb); + pub = nh.advertise(output_topic, 1); +- if(rubis::instance_mode_){ +- rubis_output_topic = "/rubis_"+output_topic.substr(1); +- pub_rubis = nh.advertise(rubis_output_topic, 1); +- } +- +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ rubis_output_topic = "/rubis_"+output_topic.substr(1); ++ pub_rubis = nh.advertise(rubis_output_topic, 1); ++ ++ // Scheduling & Profiling Setup + std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_republisher.csv"); + +- private_nh.param("/lidar_republisher/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/lidar_republisher/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/lidar_republisher/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_republisher.csv"); +- private_nh.param("/lidar_republisher/rate", rate, 10); +- private_nh.param("/lidar_republisher/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/lidar_republisher/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/lidar_republisher/task_relative_deadline", task_relative_deadline, (double)10); +- +- /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); + +- ros::spinOnce(); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- rubis::instance_ = rubis::instance_+1; +- } +- +- +- r.sleep(); +- } +- } ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + + return 0; + } +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp b/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp +index 54e6cd23..a81a3cbd 100644 +--- a/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp ++++ b/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp +@@ -8,20 +8,15 @@ ros::Subscriber rubis_sub_, sub_; + ros::Publisher rubis_pub_, pub_; + + inline void relay(const geometry_msgs::PoseStampedConstPtr& msg){ +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_msgs::PoseStamped rubis_msg; +- rubis_msg.instance = rubis::instance_; +- rubis_msg.msg = *msg; +- rubis_pub_.publish(rubis_msg); +- } ++ rubis_msgs::PoseStamped rubis_msg; ++ rubis_msg.instance = rubis::instance_; ++ rubis_msg.msg = *msg; ++ rubis_pub_.publish(rubis_msg); + pub_.publish(msg); +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; + } + + void cb(const geometry_msgs::PoseStampedConstPtr& msg){ +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + relay(msg); + } + +@@ -35,8 +30,6 @@ int main(int argc, char* argv[]){ + ros::init(argc, argv, "pose_relay"); + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; +@@ -45,65 +38,34 @@ int main(int argc, char* argv[]){ + + ros::NodeHandle nh; + +- nh.param("/pose_relay/task_scheduling_flag", task_scheduling_flag, 0); +- nh.param("/pose_relay/task_profiling_flag", task_profiling_flag, 0); + nh.param("/pose_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pose_relay.csv"); + nh.param("/pose_relay/rate", rate, 10); + nh.param("/pose_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + nh.param("/pose_relay/task_execution_time", task_execution_time, (double)10); + nh.param("/pose_relay/task_relative_deadline", task_relative_deadline, (double)10); +- nh.param("/pose_relay/instance_mode", rubis::instance_mode_, 0); + + input_topic_ = std::string(argv[1]); + + std::cout<<"!!! input topic "<("/rubis_current_pose", 10); +- } +- else{ +- rubis_sub_ = nh.subscribe(input_topic_, 10, cb); +- } ++ rubis_input_topic_ = "/rubis_"+input_topic_.substr(1); ++ rubis_sub_ = nh.subscribe(rubis_input_topic_, 10, rubis_cb); ++ rubis_pub_ = nh.advertise("/rubis_current_pose", 10); + + pub_ = nh.advertise("/current_pose", 10); + + /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ ros::Rate r(rate); ++ while(ros::ok()){ ++ rubis::start_task_profiling(); ++ ++ ros::spinOnce(); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ ++ r.sleep(); + } + + return 0; +diff --git a/rubis_ws/src/topic_tools/src/relay.cpp b/rubis_ws/src/topic_tools/src/relay.cpp +index 60862697..4a12f860 100644 +--- a/rubis_ws/src/topic_tools/src/relay.cpp ++++ b/rubis_ws/src/topic_tools/src/relay.cpp +@@ -110,10 +110,6 @@ void in_cb(const ros::MessageEvent& msg_event) + } + else + g_pub.publish(msg); +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; +- + } + + void timer_cb(const ros::TimerEvent&) +@@ -184,73 +180,10 @@ int main(int argc, char **argv) + pnh.param("monitor_rate", monitor_rate, 1.0); + monitor_timer = n.createTimer(ros::Duration(monitor_rate), &timer_cb); + } +- +- +- // scheduling +- int task_scheduling_flag = 0; +- int task_profiling_flag = 0; +- std::string task_response_time_filename; +- int rate = 0; +- double task_minimum_inter_release_time = 0; +- double task_execution_time = 0; +- double task_relative_deadline = 0; +- +- if(g_output_topic == std::string("/current_velocity")){ +- pnh.param("/vel_relay/task_scheduling_flag", task_scheduling_flag, 0); +- pnh.param("/vel_relay/task_profiling_flag", task_profiling_flag, 0); +- pnh.param("/vel_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/vel_relay.csv"); +- pnh.param("/vel_relay/rate", rate, 10); +- pnh.param("/vel_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000); +- pnh.param("/vel_relay/task_execution_time", task_execution_time, (double)100000000); +- pnh.param("/vel_relay/task_relative_deadline", task_relative_deadline, (double)100000000); +- } +- else if (g_output_topic == std::string("/current_pose")){ +- pnh.param("/pose_relay/task_scheduling_flag", task_scheduling_flag, 0); +- pnh.param("/pose_relay/task_profiling_flag", task_profiling_flag, 0); +- pnh.param("/pose_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pose_relay.csv"); +- pnh.param("/pose_relay/rate", rate, 10); +- pnh.param("/pose_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000); +- pnh.param("/pose_relay/task_execution_time", task_execution_time, (double)100000000); +- pnh.param("/pose_relay/task_relative_deadline", task_relative_deadline, (double)100000000); +- } +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); + + subscribe(); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(RUBIS_NO_INSTANCE, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ ros::spin(); + + return 0; + } +diff --git a/setup/setup.sh b/setup/setup.sh +index 50df55a6..29fb165c 100755 +--- a/setup/setup.sh ++++ b/setup/setup.sh +@@ -42,21 +42,6 @@ if [ ! -d ~/Documents/profiling/response_time ]; then + printf "~/Documents/profiling/response_time is created.\n" + fi + +-if [ ! -d ~/Documents/gpu_profiling ]; then +- mkdir ~/Documents/gpu_profiling +- printf "~/Documents/gpu_profiling is created.\n" +-fi +- +-if [ ! -d ~/Documents/gpu_profiling ]; then +- mkdir ~/Documents/gpu_profiling +- printf "~/Documents/gpu_profiling is created.\n" +-fi +- +-if [ ! -d ~/Documents/gpu_deadline ]; then +- mkdir ~/Documents/gpu_deadline +- printf "~/Documents/gpu_deadline is created.\n" +-fi +- + echo "Necessary directory paths are created to /home/${1}/Documents" + + sudo ./setup_bashrc.sh $1 diff --git a/diff3.patch b/diff3.patch new file mode 100644 index 00000000..9424560d --- /dev/null +++ b/diff3.patch @@ -0,0 +1,29869 @@ +diff --git a/autoware.ai/autoware_files/carla_launch/2_localization_param.launch b/autoware.ai/autoware_files/carla_launch/2_localization_param.launch +index f3b038fc..a712d4cf 100644 +--- a/autoware.ai/autoware_files/carla_launch/2_localization_param.launch ++++ b/autoware.ai/autoware_files/carla_launch/2_localization_param.launch +@@ -14,14 +14,12 @@ + + + +- + + + + + + +- + + + +diff --git a/autoware.ai/autoware_files/data/yaml/default_lgsvl_params.yaml b/autoware.ai/autoware_files/data/yaml/default_lgsvl_params.yaml +index 5ab0bef1..aa3fa2f0 100644 +--- a/autoware.ai/autoware_files/data/yaml/default_lgsvl_params.yaml ++++ b/autoware.ai/autoware_files/data/yaml/default_lgsvl_params.yaml +@@ -12,7 +12,7 @@ + # Sensing + ray_ground_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -22,7 +22,7 @@ ray_ground_filter: + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -33,17 +33,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + calibration_publisher: +@@ -51,37 +46,27 @@ calibration_publisher: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -91,7 +76,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -108,7 +93,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -117,7 +102,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100000000 +@@ -133,7 +118,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -146,7 +131,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 40000000 +@@ -156,7 +141,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 33333333 +@@ -167,7 +152,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -176,7 +161,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -187,7 +172,7 @@ twist_gate: + # Others + lidar_republisher: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -196,7 +181,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -205,7 +190,7 @@ vel_relay: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -214,7 +199,7 @@ vel_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml b/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml +index 4705e95b..a4354780 100644 +--- a/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml ++++ b/autoware.ai/autoware_files/data/yaml/desktop_lgsvl_params.yaml +@@ -12,7 +12,7 @@ + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -23,17 +23,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + compare_map_filter: +@@ -42,7 +37,7 @@ compare_map_filter: + max_clipping_height: 0.5 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -51,37 +46,27 @@ compare_map_filter: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -91,7 +76,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -108,7 +93,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -117,7 +102,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100000000 +@@ -133,7 +118,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -146,7 +131,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 40000000 +@@ -156,7 +141,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 33333333 +@@ -167,7 +152,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -176,7 +161,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -187,7 +172,7 @@ twist_gate: + # Others + lidar_republisher: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -196,7 +181,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -205,7 +190,7 @@ vel_relay: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -214,7 +199,7 @@ vel_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/autoware.ai/autoware_files/data/yaml/minicar_params.yaml b/autoware.ai/autoware_files/data/yaml/minicar_params.yaml +index 1a01c619..d6883c36 100644 +--- a/autoware.ai/autoware_files/data/yaml/minicar_params.yaml ++++ b/autoware.ai/autoware_files/data/yaml/minicar_params.yaml +@@ -12,7 +12,7 @@ + # Sensing + ray_ground_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -22,7 +22,7 @@ ray_ground_filter: + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -33,17 +33,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + calibration_publisher: +@@ -51,37 +46,27 @@ calibration_publisher: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 1 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -90,7 +75,7 @@ imm_ukf_pda_track: + + range_vision_fusion: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -100,7 +85,7 @@ range_vision_fusion: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -116,7 +101,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -125,7 +110,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 10 #100 + task_minimum_inter_release_time: 100000000 +@@ -141,7 +126,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 10 #100 + task_minimum_inter_release_time: 10000000 +@@ -154,7 +139,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 10 #25 + task_minimum_inter_release_time: 40000000 +@@ -164,7 +149,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 1 #30 + task_minimum_inter_release_time: 33333333 +@@ -175,7 +160,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 1 + task_minimum_inter_release_time: 100000000 +@@ -184,7 +169,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 1 + task_minimum_inter_release_time: 100000000 +diff --git a/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml b/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml +index dc83e4b8..582d2a7a 100644 +--- a/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml ++++ b/autoware.ai/autoware_files/data/yaml/nvidia_lgsvl_params.yaml +@@ -12,7 +12,7 @@ + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -23,17 +23,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + compare_map_filter: +@@ -42,7 +37,7 @@ compare_map_filter: + max_clipping_height: 0.5 + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -51,37 +46,27 @@ compare_map_filter: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -91,7 +76,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -108,7 +93,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -117,7 +102,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 10 #100 + task_minimum_inter_release_time: 100000000 +@@ -133,7 +118,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 10 #100 + task_minimum_inter_release_time: 10000000 +@@ -146,7 +131,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 10 #25 + task_minimum_inter_release_time: 40000000 +@@ -156,7 +141,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 10 #30 + task_minimum_inter_release_time: 33333333 +@@ -167,7 +152,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -176,7 +161,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -187,7 +172,7 @@ twist_gate: + # Others + lidar_republisher: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -196,7 +181,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -205,7 +190,7 @@ vel_relay: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -214,7 +199,7 @@ vel_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/autoware.ai/autoware_files/rviz/carla.rviz b/autoware.ai/autoware_files/rviz/carla.rviz +index 8257e334..3dc0c6ea 100644 +--- a/autoware.ai/autoware_files/rviz/carla.rviz ++++ b/autoware.ai/autoware_files/rviz/carla.rviz +@@ -11,7 +11,7 @@ Panels: + - /Perception1 + - /Perception1/Tracked Objects1 + Splitter Ratio: 0.4564755856990814 +- Tree Height: 547 ++ Tree Height: 408 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties +@@ -30,11 +30,11 @@ Panels: + Experimental: false + Name: Time + SyncMode: 0 +- SyncSource: Points Map ++ SyncSource: Filtered Point + - Class: autoware_rviz_debug/DecisionMakerPanel + Name: DecisionMakerPanel + - Class: integrated_viewer/ImageViewerPlugin +- Image topic: /image_raw ++ Image topic: ----- + Lane topic: ----- + Name: ImageViewerPlugin + Point size: 3 +@@ -78,10 +78,14 @@ Visualization Manager: + Value: true + ego_vehicle: + Value: true ++ ego_vehicle/collision: ++ Value: true + ego_vehicle/gnss: + Value: true + ego_vehicle/imu: + Value: true ++ ego_vehicle/lane_invasion: ++ Value: true + ego_vehicle/lidar: + Value: true + ego_vehicle/rgb_front: +@@ -96,10 +100,14 @@ Visualization Manager: + Value: true + obstacle: + Value: true ++ obstacle/collision: ++ Value: true + velodyne: + Value: true + walker: + Value: true ++ walker/collision: ++ Value: true + world: + Value: true + Marker Scale: 5 +@@ -128,9 +136,11 @@ Visualization Manager: + mobility: + {} + obstacle: +- {} ++ obstacle/collision: ++ {} + walker: +- {} ++ walker/collision: ++ {} + Update Interval: 0 + Value: true + - Alpha: 1 +@@ -352,9 +362,7 @@ Visualization Manager: + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 +- Max Intensity: 4096 + Min Color: 0; 0; 0 +- Min Intensity: 0 + Name: Points Map + Position Transformer: XYZ + Queue Size: 10 +@@ -394,9 +402,7 @@ Visualization Manager: + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 +- Max Intensity: 0.9980455636978149 + Min Color: 0; 0; 0 +- Min Intensity: 0.5593598484992981 + Name: Points Raw + Position Transformer: XYZ + Queue Size: 10 +@@ -424,9 +430,7 @@ Visualization Manager: + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 +- Max Intensity: 4096 + Min Color: 0; 0; 0 +- Min Intensity: 0 + Name: Points No Ground + Position Transformer: XYZ + Queue Size: 10 +@@ -454,9 +458,7 @@ Visualization Manager: + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 +- Max Intensity: 0.9974985122680664 + Min Color: 0; 0; 0 +- Min Intensity: 0.5666885375976562 + Name: Filtered Point + Position Transformer: XYZ + Queue Size: 10 +@@ -484,9 +486,7 @@ Visualization Manager: + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 +- Max Intensity: 4096 + Min Color: 0; 0; 0 +- Min Intensity: 0 + Name: Aligned Point + Position Transformer: XYZ + Queue Size: 10 +@@ -567,9 +567,7 @@ Visualization Manager: + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 +- Max Intensity: 4096 + Min Color: 0; 0; 0 +- Min Intensity: 0 + Name: Clustered Points + Position Transformer: XYZ + Queue Size: 10 +@@ -747,7 +745,7 @@ Visualization Manager: + Value: true + Views: + Current: +- Angle: -0.0349983349442482 ++ Angle: 0.025001663714647293 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 +@@ -757,23 +755,23 @@ Visualization Manager: + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 +- Scale: 10.921646118164062 ++ Scale: 4.204458236694336 + Target Frame: + Value: TopDownOrtho (rviz) +- X: 262.76385498046875 +- Y: 204.9719696044922 ++ X: 205.8017120361328 ++ Y: 244.97866821289062 + Saved: ~ + Window Geometry: + DecisionMakerPanel: + collapsed: false + Displays: +- collapsed: false +- Height: 1410 +- Hide Left Dock: false ++ collapsed: true ++ Height: 846 ++ Hide Left Dock: true + Hide Right Dock: true + ImageViewerPlugin: +- collapsed: false +- QMainWindow State: 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 ++ collapsed: true ++ QMainWindow State: 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 + Selection: + collapsed: false + Time: +@@ -782,6 +780,6 @@ Window Geometry: + collapsed: false + Views: + collapsed: true +- Width: 2560 +- X: 2560 +- Y: 0 ++ Width: 1865 ++ X: 44 ++ Y: 1254 +diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv +new file mode 100644 +index 00000000..3570fabd +--- /dev/null ++++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/dtlane.csv +@@ -0,0 +1,539 @@ ++DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW ++1001,0,1001,1.5706159461937805,0,90000000000,0,0,2,2 ++1002,1,1002,1.5706159461937805,0,90000000000,0,0,2,2 ++1003,2,1003,1.5710363578994468,0,2378.6207342040943,0,0,2,2 ++1004,3,1004,1.5710077368022535,0,-34939.261525991235,0,0,2,2 ++1005,4,1005,1.5711231434926902,0,8665.008902131893,0,0,2,2 ++1006,5,1006,1.5690903372401281,0,-491.93079701502967,0,0,2,2 ++1007,6,1007,1.5689557145684498,0,-7428.169323438051,0,0,2,2 ++1008,7,1008,1.5686363058159303,0,-3130.7845890626863,0,0,2,2 ++1009,8,1009,1.5682534958301988,0,-2612.2620549962444,0,0,2,2 ++1010,9,1010,1.567912565854563,0,-2933.153642870459,0,0,2,2 ++1011,10,1011,1.569028273497871,0,896.2921478559839,0,0,2,2 ++1012,11,1012,1.5683676551986594,0,-1513.7334239654833,0,0,2,2 ++1013,12,1013,1.5684754110339634,0,9280.239879158233,0,0,2,2 ++1014,13,1014,1.5684588800570125,0,-60492.49254730128,0,0,2,2 ++1015,14,1015,1.5693912035006823,0,1072.5891393053826,0,0,2,2 ++1016,15,1016,1.5515668174636283,0,-56.102914171695225,0,0,2,2 ++1017,16,1017,1.5593862076728497,0,127.88721028664114,0,0,2,2 ++1018,17,1018,1.559631749229936,0,4072.6303598717514,0,0,2,2 ++1019,18,1019,1.5603462402901191,0,1399.5976377140162,0,0,2,2 ++1020,19,1020,1.5602731051708514,0,-13673.32151792592,0,0,2,2 ++1021,20,1021,1.5605402761532297,0,3742.9214471508494,0,0,2,2 ++1022,21,1022,1.5596838425998092,0,-1167.6329074288967,0,0,2,2 ++1023,22,1023,1.5604369592339504,0,1327.8155795088473,0,0,2,2 ++1024,23,1024,1.560366573502377,0,-14207.424965920718,0,0,2,2 ++1025,24,1025,1.56027352852355,0,-10747.490220393849,0,0,2,2 ++1026,25,1026,1.5603167873838384,0,23116.65155611479,0,0,2,2 ++1027,26,1027,1.5602126432082224,0,-9602.073222867975,0,0,2,2 ++1028,27,1028,1.5603395866674272,0,7877.522845713829,0,0,2,2 ++1029,28,1029,1.5604457063862052,0,9423.319355873213,0,0,2,2 ++1030,29,1030,1.560246370088169,0,-5016.647794967425,0,0,2,2 ++1031,30,1031,1.559660768578082,0,-1707.6458697164428,0,0,2,2 ++1032,31,1032,1.5592028183805353,0,-2183.6435607133826,0,0,2,2 ++1033,32,1033,1.5581253798288566,0,-928.1271757372258,0,0,2,2 ++1034,33,1034,1.5566612631984011,0,-683.0056972230019,0,0,2,2 ++1035,34,1035,1.5555975918432194,0,-940.1400114128027,0,0,2,2 ++1036,35,1036,1.5541259437505452,0,-679.5102748938151,0,0,2,2 ++1037,36,1037,1.552885481318075,0,-806.1509754943779,0,0,2,2 ++1038,37,1038,1.5531588912852945,0,3657.5111367365535,0,0,2,2 ++1039,38,1039,1.5533266894392792,0,5959.541128748756,0,0,2,2 ++1040,39,1040,1.5541906323996943,0,1157.4838222184007,0,0,2,2 ++1041,40,1041,1.5539019306491981,0,-3463.782253766011,0,0,2,2 ++1042,41,1042,1.5537746751005848,0,-7858.203519582626,0,0,2,2 ++1043,42,1043,1.553458073462205,0,-3158.543351568176,0,0,2,2 ++1044,43,1044,1.5542071389727117,0,1334.996720545278,0,0,2,2 ++1045,44,1045,1.5542686707312687,0,16251.770198844073,0,0,2,2 ++1046,45,1046,1.5547869612148626,0,1929.4199520429686,0,0,2,2 ++1047,46,1047,1.5555746441573521,0,1269.5463441666138,0,0,2,2 ++1048,47,1048,1.5564501087977414,0,1142.250587705515,0,0,2,2 ++1049,48,1049,1.5576003889319332,0,869.3534472822894,0,0,2,2 ++1050,49,1050,1.5580756995278244,0,2103.887455159607,0,0,2,2 ++1051,50,1051,1.5585502233487594,0,2107.375764676554,0,0,2,2 ++1052,51,1052,1.5584516260756238,0,-10142.26832242346,0,0,2,2 ++1053,52,1053,1.5584112331616264,0,-24756.817496873653,0,0,2,2 ++1054,53,1054,1.5584451473870529,0,29486.151826450732,0,0,2,2 ++1055,54,1055,1.5584130588770102,0,-31163.802827570937,0,0,2,2 ++1056,55,1056,1.558453395681946,0,24791.254577367206,0,0,2,2 ++1057,56,1057,1.5584612741910804,0,126927.56750530448,0,0,2,2 ++1058,57,1058,1.558466983527938,0,175151.69010325428,0,0,2,2 ++1059,58,1059,1.5584496873239995,0,-57816.15454740148,0,0,2,2 ++1060,59,1060,1.5584670021217941,0,57754.0674664271,0,0,2,2 ++1061,60,1061,1.5584616988846198,0,-188564.07268518096,0,0,2,2 ++1062,61,1062,1.5583642287230526,0,-10259.550039937772,0,0,2,2 ++1063,62,1063,1.5584039512178554,0,25174.652422137453,0,0,2,2 ++1064,63,1064,1.5584568342185832,0,18909.668253263793,0,0,2,2 ++1065,64,1065,1.5584406089211233,0,-61632.15204384786,0,0,2,2 ++1066,65,1066,1.5584747613894863,0,29280.46047429793,0,0,2,2 ++1067,66,1067,1.5585924861497782,0,8494.389774251948,0,0,2,2 ++1068,67,1068,1.5585615744747112,0,-32350.236531457223,0,0,2,2 ++1069,68,1069,1.5584424421160337,0,-8394.025024776187,0,0,2,2 ++1070,69,1070,1.5584736998776192,0,31992.054109950375,0,0,2,2 ++1071,70,1071,1.5584616247474912,0,-82814.84252327424,0,0,2,2 ++1072,71,1072,1.55846993294119,0,120363.10614007954,0,0,2,2 ++1073,72,1073,1.558246954101369,0,-4484.730482960295,0,0,2,2 ++1074,73,1074,1.558474293779886,0,4398.704205631745,0,0,2,2 ++1075,74,1075,1.5589662998091534,0,2032.495417768197,0,0,2,2 ++1076,75,1076,1.5592385963707622,0,3672.466497892804,0,0,2,2 ++1077,76,1077,1.559661432256271,0,2364.9837543870162,0,0,2,2 ++1078,77,1078,1.5592922234671693,0,-2708.4945687052523,0,0,2,2 ++1079,78,1079,1.5593257540364545,0,29823.531819475797,0,0,2,2 ++1080,79,1080,1.5593587691302953,0,30289.17636346063,0,0,2,2 ++1081,80,1081,1.5593124961590616,0,-21610.888026868568,0,0,2,2 ++1082,81,1082,1.5594323344706573,0,8344.576844289204,0,0,2,2 ++1083,82,1083,1.55933102745173,0,-9870.984366025314,0,0,2,2 ++1084,83,1084,1.559328555630133,0,-404559.94123097276,0,0,2,2 ++1085,84,1085,1.5592803470653718,0,-20743.202062780034,0,0,2,2 ++1086,85,1086,1.5593560395197574,0,13211.356509923131,0,0,2,2 ++1087,86,1087,1.559392786498505,0,27213.12157025686,0,0,2,2 ++1088,87,1088,1.5594015836176165,0,113673.57737659848,0,0,2,2 ++1089,88,1089,1.5593621175983554,0,-25338.253482943648,0,0,2,2 ++1090,89,1090,1.5593503574191032,0,-85032.7174913827,0,0,2,2 ++1091,90,1091,1.559344540912574,0,-171924.50398892158,0,0,2,2 ++1092,91,1092,1.5593063957840103,0,-26215.6673120881,0,0,2,2 ++1093,92,1093,1.5593010517229873,0,-187123.61174189628,0,0,2,2 ++1094,93,1094,1.5593102443018543,0,108783.40174917986,0,0,2,2 ++1095,94,1095,1.5592708891791727,0,-25409.65271764711,0,0,2,2 ++1096,95,1096,1.55934742287295,0,13066.140553846302,0,0,2,2 ++1097,96,1097,1.5593046241271993,0,-23365.170695042812,0,0,2,2 ++1098,97,1098,1.5593959499423824,0,10949.806448427382,0,0,2,2 ++1099,98,1099,1.5659936909281864,0,151.56702910157335,0,0,2,2 ++1100,99,1100,1.5631484014833397,0,-351.4580921850161,0,0,2,2 ++1101,100,1101,1.5628303735240179,0,-3144.3776268368265,0,0,2,2 ++1102,101,1102,1.5623443678921973,0,-2057.58932515669,0,0,2,2 ++1103,102,1103,1.5621970301683075,0,-6787.128059257383,0,0,2,2 ++1104,103,1104,1.561518196533237,0,-1473.1149847873494,0,0,2,2 ++1105,104,1105,1.56150959557461,0,-116266.1097887333,0,0,2,2 ++1106,105,1106,1.5614280308212418,0,-12260.197679807654,0,0,2,2 ++1107,106,1107,1.5615331544577284,0,9512.608519086212,0,0,2,2 ++1108,107,1108,1.561521769820582,0,-87837.66993589127,0,0,2,2 ++1109,108,1109,1.5615063516353334,0,-64858.476134930985,0,0,2,2 ++1110,109,1110,1.5613401362473043,0,-6016.290139304405,0,0,2,2 ++1111,110,1111,1.5613462370583207,0,163912.63347054776,0,0,2,2 ++1112,111,1112,1.5616346082905865,0,3467.752286323709,0,0,2,2 ++1113,112,1113,1.5614798702693682,0,-6462.535788724596,0,0,2,2 ++1114,113,1114,1.5611376896383071,0,-2922.4330930103124,0,0,2,2 ++1115,114,1115,1.5617762833238056,0,1565.9409460327606,0,0,2,2 ++1116,115,1116,1.561901748643473,0,7970.329989601685,0,0,2,2 ++1117,116,1117,1.5619377171717277,0,27802.082779656434,0,0,2,2 ++1118,117,1118,1.5617090872820676,0,-4373.881304350305,0,0,2,2 ++1119,118,1119,1.5617712745468253,0,16080.462839074564,0,0,2,2 ++1120,119,1120,1.5620671743245305,0,3379.522647010559,0,0,2,2 ++1121,120,1121,1.562234529532306,0,5975.314501965154,0,0,2,2 ++1122,121,1122,1.5612933601741283,0,-1062.5080293055378,0,0,2,2 ++1123,122,1123,1.561698322658157,0,2469.364544715466,0,0,2,2 ++1124,123,1124,1.562024360239134,0,3067.1310865553396,0,0,2,2 ++1125,124,1125,1.5617193763911459,0,-3278.8621646585816,0,0,2,2 ++1126,125,1126,1.5617870168027121,0,14784.061433753894,0,0,2,2 ++1127,126,1127,1.5625182243896565,0,1367.6006894005648,0,0,2,2 ++1128,127,1128,1.5622360356231595,0,-3543.727173883281,0,0,2,2 ++1129,128,1129,1.5625603701776563,0,3083.2360787200023,0,0,2,2 ++1130,129,1130,1.562392918909711,0,-5971.886700355639,0,0,2,2 ++1131,130,1131,1.562318653263805,0,-13465.176095886947,0,0,2,2 ++1132,131,1132,1.562526849807251,0,4803.153709699076,0,0,2,2 ++1133,132,1133,1.576137435117669,0,73.47222600592939,0,0,2,2 ++1134,133,1134,1.5918120181402755,0,63.79755037551902,0,0,2,2 ++1135,134,1135,1.6220769767862286,0,33.04151218900524,0,0,2,2 ++1136,135,1136,1.7186597237164492,0,10.353816098464087,0,0,2,2 ++1137,136,1137,1.675493482422447,0,-23.166251450736056,0,0,2,2 ++1138,137,1138,1.677164571573219,0,598.4121191487482,0,0,2,2 ++1139,138,1139,1.6768159724168918,0,-2868.624269019426,0,0,2,2 ++1140,139,1140,1.6768476714325655,0,31546.72089172431,0,0,2,2 ++1141,140,1141,1.6905273580959745,0,73.10108956478079,0,0,2,2 ++1142,141,1142,1.7442418197685423,0,18.616960290801998,0,0,2,2 ++1143,142,1143,1.7225513490834752,0,-46.10319501680768,0,0,2,2 ++1144,143,1144,1.7485007254879514,0,38.536571531156355,0,0,2,2 ++1145,144,1145,1.8148661989890704,0,15.068076022740032,0,0,2,2 ++1146,145,1146,1.8169266574025538,0,485.32889256882964,0,0,2,2 ++1147,146,1147,1.8036604558276768,0,-75.37952701500924,0,0,2,2 ++1148,147,1148,1.8036313187476025,0,-34320.528942918514,0,0,2,2 ++1149,148,1149,1.8030076566400433,0,-1603.4323520368343,0,0,2,2 ++1150,149,1150,1.8316436098928863,0,34.921135370295964,0,0,2,2 ++1151,150,1151,1.876228798227453,0,22.428973328451868,0,0,2,2 ++1152,151,1152,1.9324085263911028,0,17.80001492864172,0,0,2,2 ++1153,152,1153,1.9158996090297133,0,-60.57332398663326,0,0,2,2 ++1154,153,1154,1.9039984847392084,0,-84.02567485139441,0,0,2,2 ++1155,154,1155,1.9849339713438288,0,12.355519710224527,0,0,2,2 ++1156,155,1156,1.954096844071843,0,-32.42844222096056,0,0,2,2 ++1157,156,1157,1.953953083593038,0,-6956.014673251483,0,0,2,2 ++1158,157,1158,2.0180868363442888,0,15.592413621554357,0,0,2,2 ++1159,158,1159,2.0020751553324976,0,-62.45440433540919,0,0,2,2 ++1160,159,1160,2.019090426427932,0,58.770735675691235,0,0,2,2 ++1161,160,1161,2.008152822027425,0,-91.42769873389011,0,0,2,2 ++1162,161,1162,2.045952035598901,0,26.455576862970116,0,0,2,2 ++1163,162,1163,2.083699401528057,0,26.491914743846912,0,0,2,2 ++1164,163,1164,2.120776070766995,0,26.971139008080037,0,0,2,2 ++1165,164,1165,2.1655019951470065,0,22.35839759293862,0,0,2,2 ++1166,165,1166,2.160816734657125,0,-213.43530464520524,0,0,2,2 ++1167,166,1167,2.2342858263354426,0,13.611165963211747,0,0,2,2 ++1168,167,1168,2.302663449689519,0,14.624667412345591,0,0,2,2 ++1169,168,1169,2.2492885285815243,0,-18.735390689883726,0,0,2,2 ++1170,169,1170,2.281647456184398,0,30.903372703587333,0,0,2,2 ++1171,170,1171,2.3457521763460143,0,15.599475319116399,0,0,2,2 ++1172,171,1172,2.474948546053671,0,7.740155565228202,0,0,2,2 ++1173,172,1173,2.5173224404688357,0,23.599435780019412,0,0,2,2 ++1174,173,1174,2.5368325471829207,0,51.255485920949084,0,0,2,2 ++1175,174,1175,2.5456673035159536,0,113.18931301602866,0,0,2,2 ++1176,175,1176,2.5547417551394753,0,110.19949650818774,0,0,2,2 ++1177,176,1177,2.603195128490749,0,20.638397924335887,0,0,2,2 ++1178,177,1178,2.596074157385382,0,-140.43028474673574,0,0,2,2 ++1179,178,1179,2.6531475137908123,0,17.521310520031932,0,0,2,2 ++1180,179,1180,2.809796039587864,0,6.383717911878494,0,0,2,2 ++1181,180,1181,2.7654462594739497,0,-22.54802611042167,0,0,2,2 ++1182,181,1182,2.771217664518214,0,173.26803305788565,0,0,2,2 ++1183,182,1183,2.8449752803934008,0,13.557921960116037,0,0,2,2 ++1184,183,1184,2.8084961628169074,0,-27.4129438000547,0,0,2,2 ++1185,184,1185,2.869009574824431,0,16.52526219932323,0,0,2,2 ++1186,185,1186,2.884302571393505,0,65.38940851018249,0,0,2,2 ++1187,186,1187,2.9905069048410624,0,9.415811648531163,0,0,2,2 ++1188,187,1188,2.969341452422632,0,-47.24680485115514,0,0,2,2 ++1189,188,1189,3.0383640323914287,0,14.488012480148827,0,0,2,2 ++1190,189,1190,3.055186812134341,0,59.44320827366907,0,0,2,2 ++1191,190,1191,3.066040854811578,0,92.13157067248235,0,0,2,2 ++1192,191,1192,3.0813008039480883,0,65.53101789883758,0,0,2,2 ++1193,192,1193,3.0910158947236637,0,102.9326460349796,0,0,2,2 ++1194,193,1194,3.10475182392835,0,72.80177300701585,0,0,2,2 ++1195,194,1195,3.0928347571304533,0,-83.91326632292699,0,0,2,2 ++1196,195,1196,3.093569046175925,0,1361.8615260122626,0,0,2,2 ++1197,196,1197,3.093684135325397,0,8688.916414664875,0,0,2,2 ++1198,197,1198,3.1063991293121975,0,78.64730420148996,0,0,2,2 ++1199,198,1199,3.1272933523549296,0,47.86011894076368,0,0,2,2 ++1200,199,1200,3.142608033171954,0,65.29682282952635,0,0,2,2 ++1201,200,1201,3.1296736144563315,0,-77.31309941220373,0,0,2,2 ++1202,201,1202,3.1295315835431703,0,-7040.7207680544525,0,0,2,2 ++1203,202,1203,3.129958005160909,0,2345.0968675165896,0,0,2,2 ++1204,203,1204,3.13035122004071,0,2543.1387553419513,0,0,2,2 ++1205,204,1205,3.130323608536339,0,-36216.78799352977,0,0,2,2 ++1206,205,1206,3.1305842969634368,0,3835.996906852997,0,0,2,2 ++1207,206,1207,3.130267219147295,0,-3153.799947810142,0,0,2,2 ++1208,207,1208,3.1302595648893865,0,-130646.23794614259,0,0,2,2 ++1209,208,1209,3.1301063347354496,0,-6526.130623167103,0,0,2,2 ++1210,209,1210,3.130327684273659,0,4517.741523609919,0,0,2,2 ++1211,210,1211,3.1304550560171522,0,7851.034872993834,0,0,2,2 ++1212,211,1212,3.130259118536645,0,-5103.668769301195,0,0,2,2 ++1213,212,1213,3.130053746211815,0,-4869.205238964958,0,0,2,2 ++1214,213,1214,3.1300164498391663,0,-26812.258913566046,0,0,2,2 ++1215,214,1215,3.129816693563907,0,-5006.100552799646,0,0,2,2 ++1216,215,1216,3.129775533359499,0,-24295.31180383525,0,0,2,2 ++1217,216,1217,3.1297243109074175,0,-19522.68896470701,0,0,2,2 ++1218,217,1218,3.1297590645461977,0,28773.965406220937,0,0,2,2 ++1219,218,1219,3.1296879225255916,0,-14056.390182348943,0,0,2,2 ++1220,219,1220,3.1297771537246764,0,11206.842564672676,0,0,2,2 ++1221,220,1221,3.1295961985796756,0,-5526.231376267536,0,0,2,2 ++1222,221,1222,3.129895503753974,0,3341.0715412600025,0,0,2,2 ++1223,222,1223,3.1304526609293504,0,1794.8256689405623,0,0,2,2 ++1224,223,1224,3.1308640539574997,0,2430.765549184491,0,0,2,2 ++1225,224,1225,3.1314005324764267,0,1864.0075319325813,0,0,2,2 ++1226,225,1226,3.131838500950461,0,2283.2693659181946,0,0,2,2 ++1227,226,1227,3.132310552198498,0,2118.414058130501,0,0,2,2 ++1228,227,1228,3.13283405273258,0,1910.2177264318536,0,0,2,2 ++1229,228,1229,3.132860414377954,0,37933.89926183089,0,0,2,2 ++1230,229,1230,3.1327026639426103,0,-6339.126721396533,0,0,2,2 ++1231,230,1231,3.1322941179223944,0,-2447.7046661023764,0,0,2,2 ++1232,231,1232,3.132958998066535,0,1504.0304764890313,0,0,2,2 ++1233,232,1233,3.1329847186775854,0,38879.32514679646,0,0,2,2 ++1234,233,1234,3.132205186132164,0,-1282.820077074694,0,0,2,2 ++1235,234,1235,3.1322812556392696,0,13145.871953777567,0,0,2,2 ++1236,235,1236,3.1326404704421495,0,2783.8496408909646,0,0,2,2 ++1237,236,1237,3.1324687366627106,0,-5822.966240347186,0,0,2,2 ++1238,237,1238,3.1327176882712133,0,4016.844904174198,0,0,2,2 ++1239,238,1239,3.1325963662922494,0,-8242.529577406198,0,0,2,2 ++1240,239,1240,3.1326772233996776,0,12367.496585104844,0,0,2,2 ++1241,240,1241,3.1324966080699,0,-5536.628597534322,0,0,2,2 ++1242,241,1242,3.1327668588296307,0,3700.2671185690515,0,0,2,2 ++1243,242,1243,3.132810249389559,0,23046.487569068846,0,0,2,2 ++1244,243,1244,3.1324986288528094,0,-3209.031119806755,0,0,2,2 ++1245,244,1245,3.132822786442226,0,3084.9192881761173,0,0,2,2 ++1246,245,1246,3.1325701209629964,0,-3957.8022413229055,0,0,2,2 ++1247,246,1247,3.132860171494029,0,3447.6751221216164,0,0,2,2 ++1248,247,1248,3.1330470784909092,0,5350.254493905768,0,0,2,2 ++1249,248,1249,3.13374161105399,0,1439.8173003782692,0,0,2,2 ++1250,249,1250,3.134233784801914,0,2031.8028017911413,0,0,2,2 ++1251,250,1251,3.133616456325006,0,-1619.8831536311138,0,0,2,2 ++1252,251,1252,3.134066252385871,0,2223.2297856879172,0,0,2,2 ++1253,252,1253,3.134784454988094,0,1392.3647685274047,0,0,2,2 ++1254,253,1254,3.1345994496722183,0,-5405.250088441288,0,0,2,2 ++1255,254,1255,3.1362019499703973,0,624.024844885431,0,0,2,2 ++1256,255,1256,3.136262484925358,0,16519.38125086305,0,0,2,2 ++1257,256,1257,3.1371188806098034,0,1167.6845390079225,0,0,2,2 ++1258,257,1258,3.1369988499566253,0,-8331.205184029783,0,0,2,2 ++1259,258,1259,3.137867554501641,0,1151.1393669315573,0,0,2,2 ++1260,259,1260,3.1708550527699093,0,30.314514664542813,0,0,2,2 ++1261,260,1261,3.2356533213820318,0,15.432511105905075,0,0,2,2 ++1262,261,1262,3.2018151731438587,0,-29.55244456527002,0,0,2,2 ++1263,262,1263,3.2823370968133814,0,12.418978017765573,0,0,2,2 ++1264,263,1264,3.36429786440126,0,12.20095942766515,0,0,2,2 ++1265,264,1265,3.4485758303486875,0,11.86549756817575,0,0,2,2 ++1266,265,1266,3.4777834180506386,0,34.23767858559573,0,0,2,2 ++1267,266,1267,3.5907094795636847,0,8.855351781523634,0,0,2,2 ++1268,267,1268,3.7473026705560817,0,6.385973704620098,0,0,2,2 ++1269,268,1269,3.757926075093773,0,94.13178199625767,0,0,2,2 ++1270,269,1270,3.8646434248411556,0,9.370547547958806,0,0,2,2 ++1271,270,1271,4.046477554099296,0,5.499517632250184,0,0,2,2 ++1272,271,1272,4.26175711340075,0,4.645122849771844,0,0,2,2 ++1273,272,1273,4.378686232163503,0,8.552189656273617,0,0,2,2 ++1274,273,1274,4.4388107350157435,0,16.63215415614419,0,0,2,2 ++1275,274,1275,4.567287183832628,0,7.783527714291725,0,0,2,2 ++1276,275,1276,4.619480219254632,0,19.15964442218305,0,0,2,2 ++1277,276,1277,4.709966137584271,0,11.051443345659726,0,0,2,2 ++1278,277,1278,4.703095070861943,0,-145.53780954425937,0,0,2,2 ++1279,278,1279,4.659772006824833,0,-23.082393229237343,0,0,2,2 ++1280,279,1280,4.665973477910038,0,161.25206201247974,0,0,2,2 ++1281,280,1281,4.636774477273602,0,-34.247747464074145,0,0,2,2 ++1282,281,1282,4.652195224894353,0,64.84769899575784,0,0,2,2 ++1283,282,1283,4.651902783891486,0,-3419.4931291989383,0,0,2,2 ++1284,283,1284,4.687824121303961,0,27.838607135287646,0,0,2,2 ++1285,284,1285,4.6736324526187385,0,-70.46387723533208,0,0,2,2 ++1286,285,1286,4.684287629155659,0,93.85109636944345,0,0,2,2 ++1287,286,1287,4.678651899887421,0,-177.43932548991228,0,0,2,2 ++1288,287,1288,4.679214309388232,0,1778.0638459293787,0,0,2,2 ++1289,288,1289,4.680302077949644,0,919.3132027110769,0,0,2,2 ++1290,289,1290,4.6973518372767735,0,58.65185430558062,0,0,2,2 ++1291,290,1291,4.690461092985631,0,-145.12220418415166,0,0,2,2 ++1292,291,1292,4.690476707440746,0,64043.22101537215,0,0,2,2 ++1293,292,1293,4.690302884709805,0,-5752.987509656575,0,0,2,2 ++1294,293,1294,4.689835859312202,0,-2141.2111742384086,0,0,2,2 ++1295,294,1295,4.689787908948702,0,-20854.899254200915,0,0,2,2 ++1296,295,1296,4.689607694324616,0,-5548.939244371828,0,0,2,2 ++1297,296,1297,4.689493209900328,0,-8734.812671833417,0,0,2,2 ++1298,297,1298,4.689519331655459,0,38282.26682969883,0,0,2,2 ++1299,298,1299,4.689388854307946,0,-7664.165612349602,0,0,2,2 ++1300,299,1300,4.68987371253164,0,2062.4585726961486,0,0,2,2 ++1301,300,1301,4.689776043791079,0,-10238.690437254516,0,0,2,2 ++1302,301,1302,4.689782693126787,0,150390.96293843692,0,0,2,2 ++1303,302,1303,4.689816670283746,0,29431.538406950272,0,0,2,2 ++1304,303,1304,4.689796642458525,0,-49930.533592485204,0,0,2,2 ++1305,304,1305,4.689747163960407,0,-20210.79939837668,0,0,2,2 ++1306,305,1306,4.689876308889746,0,7743.238585658182,0,0,2,2 ++1307,306,1307,4.689927338270111,0,19596.55384472955,0,0,2,2 ++1308,307,1308,4.689980639334232,0,18761.351513307854,0,0,2,2 ++1309,308,1309,4.6900762065868395,0,10463.835390423541,0,0,2,2 ++1310,309,1310,4.6899051812441055,0,-5847.086659870083,0,0,2,2 ++1311,310,1311,4.689963031378174,0,17286.044640952976,0,0,2,2 ++1312,311,1312,4.690015400141843,0,19095.35245718582,0,0,2,2 ++1313,312,1313,4.6899442234759725,0,-14049.548230019285,0,0,2,2 ++1314,313,1314,4.689947326133968,0,322304.2956969203,0,0,2,2 ++1315,314,1315,4.689964483759958,0,58283.12148612662,0,0,2,2 ++1316,315,1316,4.689997295188629,0,30477.18555715479,0,0,2,2 ++1317,316,1317,4.6899609756369145,0,-27533.379482733104,0,0,2,2 ++1318,317,1318,4.6898658552838945,0,-10512.997147837103,0,0,2,2 ++1319,318,1319,4.6899787594934255,0,8857.06568563471,0,0,2,2 ++1320,319,1320,4.689935576462696,0,-23157.24448047331,0,0,2,2 ++1321,320,1321,4.6899566101192764,0,47542.850963445235,0,0,2,2 ++1322,321,1322,4.689960611514752,0,249912.81320388868,0,0,2,2 ++1323,322,1323,4.690077962834115,0,8521.421023858762,0,0,2,2 ++1324,323,1324,4.690117658235953,0,25191.834663670186,0,0,2,2 ++1325,324,1325,4.690256418250002,0,7206.687076595954,0,0,2,2 ++1326,325,1326,4.690478577472271,0,4501.276110846042,0,0,2,2 ++1327,326,1327,4.690415949147022,0,-15967.216048522549,0,0,2,2 ++1328,327,1328,4.690444340102213,0,35222.48523389742,0,0,2,2 ++1329,328,1329,4.684591006921866,0,-170.84282906660815,0,0,2,2 ++1330,329,1330,4.687177878574332,0,386.567303811469,0,0,2,2 ++1331,330,1331,4.686995455840435,0,-5481.772905385729,0,0,2,2 ++1332,331,1332,4.68699624737735,0,1263364.956066549,0,0,2,2 ++1333,332,1333,4.68706285354148,0,15013.625436374265,0,0,2,2 ++1334,333,1334,4.686911557450675,0,-6609.556100751335,0,0,2,2 ++1335,334,1335,4.686865680707907,0,-21797.53704511855,0,0,2,2 ++1336,335,1336,4.686820773354471,0,-22268.067999775332,0,0,2,2 ++1337,336,1337,4.686725954049043,0,-10546.37550326547,0,0,2,2 ++1338,337,1338,4.68673837550253,0,80505.8764747865,0,0,2,2 ++1339,338,1339,4.686702762127034,0,-28079.337779137055,0,0,2,2 ++1340,339,1340,4.686710912254904,0,122697.46142708715,0,0,2,2 ++1341,340,1341,4.689136663573433,0,412.2434119118499,0,0,2,2 ++1342,341,1342,4.714259546731511,0,39.80434863736831,0,0,2,2 ++1343,342,1343,4.699984756063116,0,-70.05356668480162,0,0,2,2 ++1344,343,1344,4.6832033701032545,0,-59.58983378320814,0,0,2,2 ++1345,344,1345,4.68680269049533,0,277.83022656209147,0,0,2,2 ++1346,345,1346,4.687649092174702,0,1181.4721359512878,0,0,2,2 ++1347,346,1347,4.6876939836328555,0,22275.952734298215,0,0,2,2 ++1348,347,1348,4.687761437827265,0,14824.87499482366,0,0,2,2 ++1349,348,1349,4.687696404440299,0,-15376.717200942252,0,0,2,2 ++1350,349,1350,4.687715356450129,0,52764.85232825735,0,0,2,2 ++1351,350,1351,4.687582616571676,0,-7533.531080922668,0,0,2,2 ++1352,351,1352,4.687707352096387,0,8016.96230738979,0,0,2,2 ++1353,352,1353,4.6876684299335345,0,-25692.30296332078,0,0,2,2 ++1354,353,1354,4.687624266364536,0,-22643.09752771501,0,0,2,2 ++1355,354,1355,4.687720608966777,0,10379.624140726419,0,0,2,2 ++1356,355,1356,4.687689292770485,0,-31932.358281027387,0,0,2,2 ++1357,356,1357,4.687689884193492,0,1690837.1628970453,0,0,2,2 ++1358,357,1358,4.687685465756854,0,-226324.39519933815,0,0,2,2 ++1359,358,1359,4.687704008898185,0,53928.295217550374,0,0,2,2 ++1360,359,1360,4.6877304231527805,0,37858.34638632972,0,0,2,2 ++1361,360,1361,4.687677524239683,0,-18903.98009027763,0,0,2,2 ++1362,361,1362,4.687695679221867,0,55081.298888102705,0,0,2,2 ++1363,362,1363,4.687701035673197,0,186690.76563969703,0,0,2,2 ++1364,363,1364,4.687689154113219,0,-84164.0324843629,0,0,2,2 ++1365,364,1365,4.68770174166216,0,79443.5838693156,0,0,2,2 ++1366,365,1366,4.6876837324304255,0,-55527.0771541704,0,0,2,2 ++1367,366,1367,4.687671490794284,0,-81688.42697691922,0,0,2,2 ++1368,367,1368,4.687716206797148,0,22363.35843882257,0,0,2,2 ++1369,368,1369,4.6876577767597425,0,-17114.485021718556,0,0,2,2 ++1370,369,1370,4.687655188490774,0,-386358.6095590637,0,0,2,2 ++1371,370,1371,4.687665058282576,0,101319.25981922718,0,0,2,2 ++1372,371,1372,4.687655931651204,0,-109569.45221008976,0,0,2,2 ++1373,372,1373,4.687645642127423,0,-97186.2275966764,0,0,2,2 ++1374,373,1374,4.687697758904408,0,19187.679243744642,0,0,2,2 ++1375,374,1375,4.687658475344309,0,-25455.94130159902,0,0,2,2 ++1376,375,1376,4.687625811077754,0,-30614.494231055636,0,0,2,2 ++1377,376,1377,4.687660540456129,0,28794.065623782833,0,0,2,2 ++1378,377,1378,4.687679585735443,0,52506.44968163131,0,0,2,2 ++1379,378,1379,4.687632079348528,0,-21049.801193884927,0,0,2,2 ++1380,379,1380,4.687728014737616,0,10423.682120927764,0,0,2,2 ++1381,380,1381,4.687692515367519,0,-28169.513916645406,0,0,2,2 ++1382,381,1382,4.687670701408633,0,-45842.20614199843,0,0,2,2 ++1383,382,1383,4.6876685132754075,0,-457010.5641853212,0,0,2,2 ++1384,383,1384,4.687649360193957,0,-52210.919823960416,0,0,2,2 ++1385,384,1385,4.687743133627109,0,10664.001160973716,0,0,2,2 ++1386,385,1386,4.6876840688895305,0,-16930.5755176804,0,0,2,2 ++1387,386,1387,4.687666218497961,0,-56021.18004620228,0,0,2,2 ++1388,387,1388,4.687661596380574,0,-216351.06081571136,0,0,2,2 ++1389,388,1389,4.687720567316303,0,16957.506060121174,0,0,2,2 ++1390,389,1390,4.687640571279501,0,-12500.61928038758,0,0,2,2 ++1391,390,1391,4.687642280815317,0,584954.1089631345,0,0,2,2 ++1392,391,1392,4.6876491204650765,0,146206.3168685606,0,0,2,2 ++1393,392,1393,4.687699726107593,0,19760.64229743995,0,0,2,2 ++1394,393,1394,4.687957029171137,0,3886.467522868451,0,0,2,2 ++1395,394,1395,4.688037445066655,0,12435.352408410188,0,0,2,2 ++1396,395,1396,4.688290751141772,0,3947.793196581492,0,0,2,2 ++1397,396,1397,4.6895811702077905,0,774.9420528058007,0,0,2,2 ++1398,397,1398,4.688626779234222,0,-1047.7886188101368,0,0,2,2 ++1399,398,1399,4.689114982797937,0,2048.325891745624,0,0,2,2 ++1400,399,1400,4.689167404357547,0,19076.120730615625,0,0,2,2 ++1401,400,1401,4.68819941339195,0,-1033.067492921702,0,0,2,2 ++1402,401,1402,4.68774385311159,0,-2195.0991846980946,0,0,2,2 ++1403,402,1403,4.68775815043782,0,69943.1476822489,0,0,2,2 ++1404,403,1404,4.687759731188219,0,632610.9426376662,0,0,2,2 ++1405,404,1405,4.699177624229751,0,87.58183286203027,0,0,2,2 ++1406,405,1406,4.695531633832969,0,-274.2738984947965,0,0,2,2 ++1407,406,1407,4.695247715644894,0,-3522.1413843853197,0,0,2,2 ++1408,407,1408,4.754735647601379,0,16.810132191710583,0,0,2,2 ++1409,408,1409,4.724397302964348,0,-32.961587455216275,0,0,2,2 ++1410,409,1410,4.759533786047175,0,28.460446586036806,0,0,2,2 ++1411,410,1411,4.743882767713707,0,-63.89360607044966,0,0,2,2 ++1412,411,1412,4.744282268105393,0,2503.126456971458,0,0,2,2 ++1413,412,1413,4.745442432748384,0,861.9466263181986,0,0,2,2 ++1414,413,1414,4.813486442426927,0,14.696370844755526,0,0,2,2 ++1415,414,1415,4.776165988226798,0,-26.79495792407948,0,0,2,2 ++1416,415,1416,4.819133963999859,0,23.273146616949663,0,0,2,2 ++1417,416,1417,4.815819733304253,0,-301.72914677476547,0,0,2,2 ++1418,417,1418,4.82338051411354,0,132.26147209183162,0,0,2,2 ++1419,418,1419,4.883073471345198,0,16.752395029101688,0,0,2,2 ++1420,419,1420,4.884712249460814,0,610.2107359567673,0,0,2,2 ++1421,420,1421,4.961541038870127,0,13.015954145423295,0,0,2,2 ++1422,421,1422,4.999906861541805,0,26.064865298410677,0,0,2,2 ++1423,422,1423,4.993773766242908,0,-163.04980621772725,0,0,2,2 ++1424,423,1424,5.027284605822452,0,29.841090600739474,0,0,2,2 ++1425,424,1425,5.049561561017202,0,44.8894380429352,0,0,2,2 ++1426,425,1426,5.068208436003781,0,53.62828896100618,0,0,2,2 ++1427,426,1427,5.050902273466685,0,-57.78288501893302,0,0,2,2 ++1428,427,1428,5.071232352781532,0,49.18819963824127,0,0,2,2 ++1429,428,1429,5.099616439035059,0,35.23100906148571,0,0,2,2 ++1430,429,1430,5.0834811075303215,0,-61.975795149072646,0,0,2,2 ++1431,430,1431,5.198289675352311,0,8.710151332525102,0,0,2,2 ++1432,431,1432,5.157266264870209,0,-24.37632532859001,0,0,2,2 ++1433,432,1433,5.2054644475826946,0,20.747670217469807,0,0,2,2 ++1434,433,1434,5.211499235975039,0,165.7058930630452,0,0,2,2 ++1435,434,1435,5.201878966277255,0,-103.9471897789248,0,0,2,2 ++1436,435,1436,5.278536273467066,0,13.045070804847205,0,0,2,2 ++1437,436,1437,5.299850103443958,0,46.91789326855794,0,0,2,2 ++1438,437,1438,5.292430313476906,0,-134.7747044647677,0,0,2,2 ++1439,438,1439,5.335799600526837,0,23.057792000332128,0,0,2,2 ++1440,439,1440,5.38607544957814,0,19.890265781082434,0,0,2,2 ++1441,440,1441,5.354411048658623,0,-31.581207000939052,0,0,2,2 ++1442,441,1442,5.354553509767046,0,7019.459634075851,0,0,2,2 ++1443,442,1443,5.418189239753329,0,15.71444218861867,0,0,2,2 ++1444,443,1444,5.478070005053282,0,16.69985336678365,0,0,2,2 ++1445,444,1445,5.457578047651754,0,-48.79963296846651,0,0,2,2 ++1446,445,1446,5.44743639717364,0,-98.60327982689128,0,0,2,2 ++1447,446,1447,5.447582422152287,0,6848.143442770463,0,0,2,2 ++1448,447,1448,5.552216302498086,0,9.557133852774605,0,0,2,2 ++1449,448,1449,5.502154534883066,0,-19.975323438239375,0,0,2,2 ++1450,449,1450,5.594327596757653,0,10.849156789004411,0,0,2,2 ++1451,450,1451,5.703767870137007,0,9.13740407549686,0,0,2,2 ++1452,451,1452,5.790683385369762,0,11.505425669077031,0,0,2,2 ++1453,452,1453,5.851272330781886,0,16.50466092779857,0,0,2,2 ++1454,453,1454,5.784744718309785,0,-15.031352589413242,0,0,2,2 ++1455,454,1455,5.866744337751296,0,12.195178548520975,0,0,2,2 ++1456,455,1456,5.83407175340783,0,-30.606700390995794,0,0,2,2 ++1457,456,1457,5.83298837237763,0,-923.0362837492128,0,0,2,2 ++1458,457,1458,5.909277584800559,0,13.108013154680902,0,0,2,2 ++1459,458,1459,5.885308774937403,0,-41.720886673524866,0,0,2,2 ++1460,459,1460,5.919908712922938,0,28.901785905456688,0,0,2,2 ++1461,460,1461,5.965351169098282,0,22.00585276775979,0,0,2,2 ++1462,461,1462,6.016863401140053,0,19.412864874290744,0,0,2,2 ++1463,462,1463,6.060070083297614,0,23.144568156224413,0,0,2,2 ++1464,463,1464,6.124157266259176,0,15.603744052843995,0,0,2,2 ++1465,464,1465,6.119357144906505,0,-208.3280664235001,0,0,2,2 ++1466,465,1466,6.130669368179921,0,88.39995249652004,0,0,2,2 ++1467,466,1467,6.180864201080793,0,19.922369339786027,0,0,2,2 ++1468,467,1468,6.230570579446633,0,20.11814243717314,0,0,2,2 ++1469,468,1469,6.263350807100293,0,30.506194483013054,0,0,2,2 ++1470,469,1470,6.193143317898733,0,-14.243494695118356,0,0,2,2 ++1471,470,1471,6.195834121923333,0,371.6361321217441,0,0,2,2 ++1472,471,1472,6.208803875641413,0,77.1024663796037,0,0,2,2 ++1473,472,1473,6.221901699465272,0,76.34856090966778,0,0,2,2 ++1474,473,1474,6.256501196726211,0,28.9021540532308,0,0,2,2 ++1475,474,1475,6.23629215057456,0,-49.48279065205987,0,0,2,2 ++1476,475,1476,6.235907617862001,0,-2600.5589832538453,0,0,2,2 ++1477,476,1477,6.237021459432858,0,897.7937492762666,0,0,2,2 ++1478,477,1478,6.237655749087532,0,1576.5667824346535,0,0,2,2 ++1479,478,1479,6.244179133136743,0,153.29466921710022,0,0,2,2 ++1480,479,1480,6.270392764015719,0,38.1480919074826,0,0,2,2 ++1481,480,1481,6.256087261027579,0,-69.903169488625,0,0,2,2 ++1482,481,1482,6.257661319758907,0,635.3003100185017,0,0,2,2 ++1483,482,1483,6.267836945548009,0,98.27405416883752,0,0,2,2 ++1484,483,1484,6.269723566488711,0,530.0481821365647,0,0,2,2 ++1485,484,1485,6.267054253560375,0,-374.6282383697463,0,0,2,2 ++1486,485,1486,0.013744254559647902,0,-0.15991530887798605,0,0,2,2 ++1487,486,1487,0.0010119061751368357,0,-78.54010664807936,0,0,2,2 ++1488,487,1488,0.0012243999420399345,0,4706.0203909699585,0,0,2,2 ++1489,488,1489,0.0018815871511780942,0,1521.6364318949657,0,0,2,2 ++1490,489,1490,0.002008029429596202,0,7908.747078198724,0,0,2,2 ++1491,490,1491,0.0018352499454211354,0,-5787.724189445794,0,0,2,2 ++1492,491,1492,0.0011422250006752636,0,-1442.9495035949885,0,0,2,2 ++1493,492,1493,0.001342209822478479,0,5000.379483719008,0,0,2,2 ++1494,493,1494,6.27518733217875,0,0.1593918849600848,0,0,2,2 ++1495,494,1495,6.277509724011835,0,430.5905600226852,0,0,2,2 ++1496,495,1496,6.278068934614515,0,1788.2350499208055,0,0,2,2 ++1497,496,1497,6.27766295708146,0,-2463.1904935097405,0,0,2,2 ++1498,497,1498,6.277751071486938,0,11348.882110444736,0,0,2,2 ++1499,498,1499,6.277640581367299,0,-9050.58301386519,0,0,2,2 ++1500,499,1500,6.2774103081305626,0,-4342.667060107362,0,0,2,2 ++1501,500,1501,6.267469573203085,0,-100.59618401410951,0,0,2,2 ++1502,501,1502,6.268776934010963,0,764.899784339935,0,0,2,2 ++1503,502,1503,6.269099769240327,0,3097.5553751321304,0,0,2,2 ++1504,503,1504,6.269088227498937,0,-86642.03833671357,0,0,2,2 ++1505,504,1505,6.269037911255563,0,-19874.297700944935,0,0,2,2 ++1506,505,1506,6.26957021841891,0,1878.6145835655457,0,0,2,2 ++1507,506,1507,6.268937417154099,0,-1580.2749703702154,0,0,2,2 ++1508,507,1508,6.25233126629231,0,-60.21865080733453,0,0,2,2 ++1509,508,1509,6.258024899264237,0,175.634784491826,0,0,2,2 ++1510,509,1510,6.230735642842278,0,-36.644457604030876,0,0,2,2 ++1511,510,1511,6.240254079440697,0,105.05926993999557,0,0,2,2 ++1512,511,1512,6.239790331359758,0,-2156.343155047787,0,0,2,2 ++1513,512,1513,6.239202869296887,0,-1702.2375795858684,0,0,2,2 ++1514,513,1514,6.238970234292401,0,-4298.579236645538,0,0,2,2 ++1515,514,1515,6.238925065169713,0,-22139.017552296125,0,0,2,2 ++1516,515,1516,6.238863666976653,0,-16287.124264657097,0,0,2,2 ++1517,516,1517,6.238732914238007,0,-7648.023363449819,0,0,2,2 ++1518,517,1518,6.264384233905774,0,38.98434906865983,0,0,2,2 ++1519,518,1519,6.254531263325934,0,-101.49223443801617,0,0,2,2 ++1520,519,1520,0.010624767483427618,0,-0.1601561459425839,0,0,2,2 ++1521,520,1521,6.272689144792406,0,0.1596917469618436,0,0,2,2 ++1522,521,1522,6.272761513075674,0,13818.208127091919,0,0,2,2 ++1523,522,1523,6.272970784747892,0,4778.477609518447,0,0,2,2 ++1524,523,1524,6.27296208880721,0,-114996.18460447,0,0,2,2 ++1525,524,1525,6.273040252520847,0,12793.660299308754,0,0,2,2 ++1526,525,1526,6.273027181376255,0,-76504.3943162592,0,0,2,2 ++1527,526,1527,6.272997881943862,0,-34130.35401441668,0,0,2,2 ++1528,527,1528,6.264496571896351,0,-117.62892947220149,0,0,2,2 ++1529,528,1529,6.256412251422299,0,-123.69623436004015,0,0,2,2 ++1530,529,1530,6.262334255024012,0,168.86176828916354,0,0,2,2 ++1531,530,1531,6.262342165248228,0,126418.66686483091,0,0,2,2 ++1532,531,1532,6.262333893420006,0,-120892.25902943166,0,0,2,2 ++1533,532,1533,6.262315140960322,0,-53326.33781473414,0,0,2,2 ++1534,533,1534,6.2621862311902685,0,-7757.363926623419,0,0,2,2 ++1535,534,1535,6.262360065436234,0,5752.60642368762,0,0,2,2 ++1536,535,1536,6.26219896741017,0,-6207.400701499976,0,0,2,2 ++1537,536,1537,6.262701530291994,0,1989.8007516411894,0,0,2,2 ++1538,537,1538,6.263282807370463,0,1720.3499622483916,0,0,2,2 +diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv +new file mode 100644 +index 00000000..045416d0 +--- /dev/null ++++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/lane.csv +@@ -0,0 +1,538 @@ ++LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,CrossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG ++1001,1001,0,1002,1001,1002,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1002,1002,1001,1003,1002,1003,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1003,1003,1002,1004,1003,1004,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1004,1004,1003,1005,1004,1005,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1005,1005,1004,1006,1005,1006,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1006,1006,1005,1007,1006,1007,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1007,1007,1006,1008,1007,1008,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1008,1008,1007,1009,1008,1009,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1009,1009,1008,1010,1009,1010,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1010,1010,1009,1011,1010,1011,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1011,1011,1010,1012,1011,1012,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1012,1012,1011,1013,1012,1013,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1013,1013,1012,1014,1013,1014,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1014,1014,1013,1015,1014,1015,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1015,1015,1014,1016,1015,1016,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1016,1016,1015,1017,1016,1017,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1017,1017,1016,1018,1017,1018,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1018,1018,1017,1019,1018,1019,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1019,1019,1018,1020,1019,1020,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1020,1020,1019,1021,1020,1021,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1021,1021,1020,1022,1021,1022,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1022,1022,1021,1023,1022,1023,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1023,1023,1022,1024,1023,1024,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1024,1024,1023,1025,1024,1025,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1025,1025,1024,1026,1025,1026,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1026,1026,1025,1027,1026,1027,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1027,1027,1026,1028,1027,1028,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1028,1028,1027,1029,1028,1029,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1029,1029,1028,1030,1029,1030,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1030,1030,1029,1031,1030,1031,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1031,1031,1030,1032,1031,1032,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1032,1032,1031,1033,1032,1033,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1033,1033,1032,1034,1033,1034,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1034,1034,1033,1035,1034,1035,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1035,1035,1034,1036,1035,1036,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1036,1036,1035,1037,1036,1037,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1037,1037,1036,1038,1037,1038,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1038,1038,1037,1039,1038,1039,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1039,1039,1038,1040,1039,1040,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1040,1040,1039,1041,1040,1041,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1041,1041,1040,1042,1041,1042,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1042,1042,1041,1043,1042,1043,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1043,1043,1042,1044,1043,1044,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1044,1044,1043,1045,1044,1045,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1045,1045,1044,1046,1045,1046,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1046,1046,1045,1047,1046,1047,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1047,1047,1046,1048,1047,1048,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1048,1048,1047,1049,1048,1049,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1049,1049,1048,1050,1049,1050,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1050,1050,1049,1051,1050,1051,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1051,1051,1050,1052,1051,1052,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1052,1052,1051,1053,1052,1053,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1053,1053,1052,1054,1053,1054,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1054,1054,1053,1055,1054,1055,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1055,1055,1054,1056,1055,1056,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1056,1056,1055,1057,1056,1057,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1057,1057,1056,1058,1057,1058,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1058,1058,1057,1059,1058,1059,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1059,1059,1058,1060,1059,1060,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1060,1060,1059,1061,1060,1061,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1061,1061,1060,1062,1061,1062,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1062,1062,1061,1063,1062,1063,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1063,1063,1062,1064,1063,1064,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1064,1064,1063,1065,1064,1065,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1065,1065,1064,1066,1065,1066,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1066,1066,1065,1067,1066,1067,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1067,1067,1066,1068,1067,1068,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1068,1068,1067,1069,1068,1069,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1069,1069,1068,1070,1069,1070,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1070,1070,1069,1071,1070,1071,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1071,1071,1070,1072,1071,1072,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1072,1072,1071,1073,1072,1073,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1073,1073,1072,1074,1073,1074,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1074,1074,1073,1075,1074,1075,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1075,1075,1074,1076,1075,1076,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1076,1076,1075,1077,1076,1077,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1077,1077,1076,1078,1077,1078,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1078,1078,1077,1079,1078,1079,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1079,1079,1078,1080,1079,1080,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1080,1080,1079,1081,1080,1081,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1081,1081,1080,1082,1081,1082,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1082,1082,1081,1083,1082,1083,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1083,1083,1082,1084,1083,1084,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1084,1084,1083,1085,1084,1085,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1085,1085,1084,1086,1085,1086,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1086,1086,1085,1087,1086,1087,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1087,1087,1086,1088,1087,1088,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1088,1088,1087,1089,1088,1089,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1089,1089,1088,1090,1089,1090,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1090,1090,1089,1091,1090,1091,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1091,1091,1090,1092,1091,1092,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1092,1092,1091,1093,1092,1093,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1093,1093,1092,1094,1093,1094,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1094,1094,1093,1095,1094,1095,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1095,1095,1094,1096,1095,1096,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1096,1096,1095,1097,1096,1097,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1097,1097,1096,1098,1097,1098,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1098,1098,1097,1099,1098,1099,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1099,1099,1098,1100,1099,1100,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1100,1100,1099,1101,1100,1101,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1101,1101,1100,1102,1101,1102,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1102,1102,1101,1103,1102,1103,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1103,1103,1102,1104,1103,1104,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1104,1104,1103,1105,1104,1105,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1105,1105,1104,1106,1105,1106,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1106,1106,1105,1107,1106,1107,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1107,1107,1106,1108,1107,1108,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1108,1108,1107,1109,1108,1109,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1109,1109,1108,1110,1109,1110,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1110,1110,1109,1111,1110,1111,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1111,1111,1110,1112,1111,1112,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1112,1112,1111,1113,1112,1113,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1113,1113,1112,1114,1113,1114,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1114,1114,1113,1115,1114,1115,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1115,1115,1114,1116,1115,1116,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1116,1116,1115,1117,1116,1117,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1117,1117,1116,1118,1117,1118,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1118,1118,1117,1119,1118,1119,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1119,1119,1118,1120,1119,1120,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1120,1120,1119,1121,1120,1121,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1121,1121,1120,1122,1121,1122,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1122,1122,1121,1123,1122,1123,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1123,1123,1122,1124,1123,1124,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1124,1124,1123,1125,1124,1125,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1125,1125,1124,1126,1125,1126,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1126,1126,1125,1127,1126,1127,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1127,1127,1126,1128,1127,1128,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1128,1128,1127,1129,1128,1129,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1129,1129,1128,1130,1129,1130,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1130,1130,1129,1131,1130,1131,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1131,1131,1130,1132,1131,1132,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1132,1132,1131,1133,1132,1133,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1133,1133,1132,1134,1133,1134,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1134,1134,1133,1135,1134,1135,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1135,1135,1134,1136,1135,1136,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1136,1136,1135,1137,1136,1137,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1137,1137,1136,1138,1137,1138,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1138,1138,1137,1139,1138,1139,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1139,1139,1138,1140,1139,1140,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1140,1140,1139,1141,1140,1141,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1141,1141,1140,1142,1141,1142,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1142,1142,1141,1143,1142,1143,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1143,1143,1142,1144,1143,1144,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1144,1144,1143,1145,1144,1145,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1145,1145,1144,1146,1145,1146,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1146,1146,1145,1147,1146,1147,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1147,1147,1146,1148,1147,1148,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1148,1148,1147,1149,1148,1149,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1149,1149,1148,1150,1149,1150,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1150,1150,1149,1151,1150,1151,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1151,1151,1150,1152,1151,1152,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1152,1152,1151,1153,1152,1153,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1153,1153,1152,1154,1153,1154,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1154,1154,1153,1155,1154,1155,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1155,1155,1154,1156,1155,1156,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1156,1156,1155,1157,1156,1157,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1157,1157,1156,1158,1157,1158,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1158,1158,1157,1159,1158,1159,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1159,1159,1158,1160,1159,1160,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1160,1160,1159,1161,1160,1161,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1161,1161,1160,1162,1161,1162,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1162,1162,1161,1163,1162,1163,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1163,1163,1162,1164,1163,1164,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1164,1164,1163,1165,1164,1165,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1165,1165,1164,1166,1165,1166,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1166,1166,1165,1167,1166,1167,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1167,1167,1166,1168,1167,1168,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1168,1168,1167,1169,1168,1169,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1169,1169,1168,1170,1169,1170,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1170,1170,1169,1171,1170,1171,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1171,1171,1170,1172,1171,1172,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1172,1172,1171,1173,1172,1173,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1173,1173,1172,1174,1173,1174,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1174,1174,1173,1175,1174,1175,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1175,1175,1174,1176,1175,1176,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1176,1176,1175,1177,1176,1177,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1177,1177,1176,1178,1177,1178,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1178,1178,1177,1179,1178,1179,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1179,1179,1178,1180,1179,1180,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1180,1180,1179,1181,1180,1181,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1181,1181,1180,1182,1181,1182,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1182,1182,1181,1183,1182,1183,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1183,1183,1182,1184,1183,1184,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1184,1184,1183,1185,1184,1185,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1185,1185,1184,1186,1185,1186,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1186,1186,1185,1187,1186,1187,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1187,1187,1186,1188,1187,1188,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1188,1188,1187,1189,1188,1189,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1189,1189,1188,1190,1189,1190,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1190,1190,1189,1191,1190,1191,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1191,1191,1190,1192,1191,1192,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1192,1192,1191,1193,1192,1193,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1193,1193,1192,1194,1193,1194,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1194,1194,1193,1195,1194,1195,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1195,1195,1194,1196,1195,1196,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1196,1196,1195,1197,1196,1197,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1197,1197,1196,1198,1197,1198,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1198,1198,1197,1199,1198,1199,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1199,1199,1198,1200,1199,1200,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1200,1200,1199,1201,1200,1201,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1201,1201,1200,1202,1201,1202,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1202,1202,1201,1203,1202,1203,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1203,1203,1202,1204,1203,1204,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1204,1204,1203,1205,1204,1205,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1205,1205,1204,1206,1205,1206,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1206,1206,1205,1207,1206,1207,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1207,1207,1206,1208,1207,1208,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1208,1208,1207,1209,1208,1209,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1209,1209,1208,1210,1209,1210,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1210,1210,1209,1211,1210,1211,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1211,1211,1210,1212,1211,1212,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1212,1212,1211,1213,1212,1213,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1213,1213,1212,1214,1213,1214,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1214,1214,1213,1215,1214,1215,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1215,1215,1214,1216,1215,1216,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1216,1216,1215,1217,1216,1217,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1217,1217,1216,1218,1217,1218,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1218,1218,1217,1219,1218,1219,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1219,1219,1218,1220,1219,1220,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1220,1220,1219,1221,1220,1221,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1221,1221,1220,1222,1221,1222,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1222,1222,1221,1223,1222,1223,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1223,1223,1222,1224,1223,1224,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1224,1224,1223,1225,1224,1225,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1225,1225,1224,1226,1225,1226,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1226,1226,1225,1227,1226,1227,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1227,1227,1226,1228,1227,1228,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1228,1228,1227,1229,1228,1229,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1229,1229,1228,1230,1229,1230,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1230,1230,1229,1231,1230,1231,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1231,1231,1230,1232,1231,1232,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1232,1232,1231,1233,1232,1233,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1233,1233,1232,1234,1233,1234,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1234,1234,1233,1235,1234,1235,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1235,1235,1234,1236,1235,1236,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1236,1236,1235,1237,1236,1237,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1237,1237,1236,1238,1237,1238,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1238,1238,1237,1239,1238,1239,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1239,1239,1238,1240,1239,1240,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1240,1240,1239,1241,1240,1241,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1241,1241,1240,1242,1241,1242,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1242,1242,1241,1243,1242,1243,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1243,1243,1242,1244,1243,1244,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1244,1244,1243,1245,1244,1245,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1245,1245,1244,1246,1245,1246,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1246,1246,1245,1247,1246,1247,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1247,1247,1246,1248,1247,1248,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1248,1248,1247,1249,1248,1249,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1249,1249,1248,1250,1249,1250,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1250,1250,1249,1251,1250,1251,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1251,1251,1250,1252,1251,1252,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1252,1252,1251,1253,1252,1253,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1253,1253,1252,1254,1253,1254,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1254,1254,1253,1255,1254,1255,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1255,1255,1254,1256,1255,1256,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1256,1256,1255,1257,1256,1257,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1257,1257,1256,1258,1257,1258,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1258,1258,1257,1259,1258,1259,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1259,1259,1258,1260,1259,1260,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1260,1260,1259,1261,1260,1261,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1261,1261,1260,1262,1261,1262,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1262,1262,1261,1263,1262,1263,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1263,1263,1262,1264,1263,1264,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1264,1264,1263,1265,1264,1265,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1265,1265,1264,1266,1265,1266,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1266,1266,1265,1267,1266,1267,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1267,1267,1266,1268,1267,1268,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1268,1268,1267,1269,1268,1269,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1269,1269,1268,1270,1269,1270,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1270,1270,1269,1271,1270,1271,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1271,1271,1270,1272,1271,1272,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1272,1272,1271,1273,1272,1273,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1273,1273,1272,1274,1273,1274,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1274,1274,1273,1275,1274,1275,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1275,1275,1274,1276,1275,1276,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1276,1276,1275,1277,1276,1277,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1277,1277,1276,1278,1277,1278,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1278,1278,1277,1279,1278,1279,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1279,1279,1278,1280,1279,1280,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1280,1280,1279,1281,1280,1281,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1281,1281,1280,1282,1281,1282,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1282,1282,1281,1283,1282,1283,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1283,1283,1282,1284,1283,1284,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1284,1284,1283,1285,1284,1285,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1285,1285,1284,1286,1285,1286,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1286,1286,1285,1287,1286,1287,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1287,1287,1286,1288,1287,1288,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1288,1288,1287,1289,1288,1289,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1289,1289,1288,1290,1289,1290,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1290,1290,1289,1291,1290,1291,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1291,1291,1290,1292,1291,1292,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1292,1292,1291,1293,1292,1293,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1293,1293,1292,1294,1293,1294,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1294,1294,1293,1295,1294,1295,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1295,1295,1294,1296,1295,1296,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1296,1296,1295,1297,1296,1297,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1297,1297,1296,1298,1297,1298,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1298,1298,1297,1299,1298,1299,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1299,1299,1298,1300,1299,1300,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1300,1300,1299,1301,1300,1301,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1301,1301,1300,1302,1301,1302,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1302,1302,1301,1303,1302,1303,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1303,1303,1302,1304,1303,1304,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1304,1304,1303,1305,1304,1305,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1305,1305,1304,1306,1305,1306,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1306,1306,1305,1307,1306,1307,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1307,1307,1306,1308,1307,1308,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1308,1308,1307,1309,1308,1309,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1309,1309,1308,1310,1309,1310,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1310,1310,1309,1311,1310,1311,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1311,1311,1310,1312,1311,1312,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1312,1312,1311,1313,1312,1313,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1313,1313,1312,1314,1313,1314,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1314,1314,1313,1315,1314,1315,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1315,1315,1314,1316,1315,1316,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1316,1316,1315,1317,1316,1317,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1317,1317,1316,1318,1317,1318,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1318,1318,1317,1319,1318,1319,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1319,1319,1318,1320,1319,1320,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1320,1320,1319,1321,1320,1321,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1321,1321,1320,1322,1321,1322,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1322,1322,1321,1323,1322,1323,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1323,1323,1322,1324,1323,1324,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1324,1324,1323,1325,1324,1325,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1325,1325,1324,1326,1325,1326,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1326,1326,1325,1327,1326,1327,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1327,1327,1326,1328,1327,1328,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1328,1328,1327,1329,1328,1329,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1329,1329,1328,1330,1329,1330,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1330,1330,1329,1331,1330,1331,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1331,1331,1330,1332,1331,1332,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1332,1332,1331,1333,1332,1333,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1333,1333,1332,1334,1333,1334,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1334,1334,1333,1335,1334,1335,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1335,1335,1334,1336,1335,1336,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1336,1336,1335,1337,1336,1337,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1337,1337,1336,1338,1337,1338,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1338,1338,1337,1339,1338,1339,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1339,1339,1338,1340,1339,1340,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1340,1340,1339,1341,1340,1341,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1341,1341,1340,1342,1341,1342,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1342,1342,1341,1343,1342,1343,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1343,1343,1342,1344,1343,1344,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1344,1344,1343,1345,1344,1345,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1345,1345,1344,1346,1345,1346,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1346,1346,1345,1347,1346,1347,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1347,1347,1346,1348,1347,1348,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1348,1348,1347,1349,1348,1349,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1349,1349,1348,1350,1349,1350,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1350,1350,1349,1351,1350,1351,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1351,1351,1350,1352,1351,1352,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1352,1352,1351,1353,1352,1353,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1353,1353,1352,1354,1353,1354,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1354,1354,1353,1355,1354,1355,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1355,1355,1354,1356,1355,1356,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1356,1356,1355,1357,1356,1357,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1357,1357,1356,1358,1357,1358,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1358,1358,1357,1359,1358,1359,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1359,1359,1358,1360,1359,1360,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1360,1360,1359,1361,1360,1361,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1361,1361,1360,1362,1361,1362,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1362,1362,1361,1363,1362,1363,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1363,1363,1362,1364,1363,1364,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1364,1364,1363,1365,1364,1365,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1365,1365,1364,1366,1365,1366,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1366,1366,1365,1367,1366,1367,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1367,1367,1366,1368,1367,1368,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1368,1368,1367,1369,1368,1369,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1369,1369,1368,1370,1369,1370,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1370,1370,1369,1371,1370,1371,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1371,1371,1370,1372,1371,1372,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1372,1372,1371,1373,1372,1373,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1373,1373,1372,1374,1373,1374,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1374,1374,1373,1375,1374,1375,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1375,1375,1374,1376,1375,1376,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1376,1376,1375,1377,1376,1377,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1377,1377,1376,1378,1377,1378,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1378,1378,1377,1379,1378,1379,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1379,1379,1378,1380,1379,1380,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1380,1380,1379,1381,1380,1381,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1381,1381,1380,1382,1381,1382,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1382,1382,1381,1383,1382,1383,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1383,1383,1382,1384,1383,1384,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1384,1384,1383,1385,1384,1385,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1385,1385,1384,1386,1385,1386,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1386,1386,1385,1387,1386,1387,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1387,1387,1386,1388,1387,1388,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1388,1388,1387,1389,1388,1389,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1389,1389,1388,1390,1389,1390,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1390,1390,1389,1391,1390,1391,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1391,1391,1390,1392,1391,1392,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1392,1392,1391,1393,1392,1393,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1393,1393,1392,1394,1393,1394,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1394,1394,1393,1395,1394,1395,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1395,1395,1394,1396,1395,1396,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1396,1396,1395,1397,1396,1397,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1397,1397,1396,1398,1397,1398,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1398,1398,1397,1399,1398,1399,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1399,1399,1398,1400,1399,1400,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1400,1400,1399,1401,1400,1401,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1401,1401,1400,1402,1401,1402,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1402,1402,1401,1403,1402,1403,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1403,1403,1402,1404,1403,1404,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1404,1404,1403,1405,1404,1405,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1405,1405,1404,1406,1405,1406,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1406,1406,1405,1407,1406,1407,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1407,1407,1406,1408,1407,1408,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1408,1408,1407,1409,1408,1409,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1409,1409,1408,1410,1409,1410,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1410,1410,1409,1411,1410,1411,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1411,1411,1410,1412,1411,1412,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1412,1412,1411,1413,1412,1413,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1413,1413,1412,1414,1413,1414,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1414,1414,1413,1415,1414,1415,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1415,1415,1414,1416,1415,1416,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1416,1416,1415,1417,1416,1417,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1417,1417,1416,1418,1417,1418,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1418,1418,1417,1419,1418,1419,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1419,1419,1418,1420,1419,1420,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1420,1420,1419,1421,1420,1421,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1421,1421,1420,1422,1421,1422,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1422,1422,1421,1423,1422,1423,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1423,1423,1422,1424,1423,1424,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1424,1424,1423,1425,1424,1425,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1425,1425,1424,1426,1425,1426,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1426,1426,1425,1427,1426,1427,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1427,1427,1426,1428,1427,1428,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1428,1428,1427,1429,1428,1429,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1429,1429,1428,1430,1429,1430,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1430,1430,1429,1431,1430,1431,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1431,1431,1430,1432,1431,1432,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1432,1432,1431,1433,1432,1433,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1433,1433,1432,1434,1433,1434,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1434,1434,1433,1435,1434,1435,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1435,1435,1434,1436,1435,1436,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1436,1436,1435,1437,1436,1437,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1437,1437,1436,1438,1437,1438,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1438,1438,1437,1439,1438,1439,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1439,1439,1438,1440,1439,1440,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1440,1440,1439,1441,1440,1441,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1441,1441,1440,1442,1441,1442,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1442,1442,1441,1443,1442,1443,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1443,1443,1442,1444,1443,1444,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1444,1444,1443,1445,1444,1445,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1445,1445,1444,1446,1445,1446,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1446,1446,1445,1447,1446,1447,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1447,1447,1446,1448,1447,1448,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1448,1448,1447,1449,1448,1449,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1449,1449,1448,1450,1449,1450,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1450,1450,1449,1451,1450,1451,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1451,1451,1450,1452,1451,1452,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1452,1452,1451,1453,1452,1453,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1453,1453,1452,1454,1453,1454,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1454,1454,1453,1455,1454,1455,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1455,1455,1454,1456,1455,1456,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1456,1456,1455,1457,1456,1457,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1457,1457,1456,1458,1457,1458,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1458,1458,1457,1459,1458,1459,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1459,1459,1458,1460,1459,1460,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1460,1460,1459,1461,1460,1461,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1461,1461,1460,1462,1461,1462,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1462,1462,1461,1463,1462,1463,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1463,1463,1462,1464,1463,1464,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1464,1464,1463,1465,1464,1465,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1465,1465,1464,1466,1465,1466,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1466,1466,1465,1467,1466,1467,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1467,1467,1466,1468,1467,1468,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1468,1468,1467,1469,1468,1469,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1469,1469,1468,1470,1469,1470,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1470,1470,1469,1471,1470,1471,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1471,1471,1470,1472,1471,1472,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1472,1472,1471,1473,1472,1473,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1473,1473,1472,1474,1473,1474,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1474,1474,1473,1475,1474,1475,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1475,1475,1474,1476,1475,1476,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1476,1476,1475,1477,1476,1477,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1477,1477,1476,1478,1477,1478,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1478,1478,1477,1479,1478,1479,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1479,1479,1478,1480,1479,1480,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1480,1480,1479,1481,1480,1481,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1481,1481,1480,1482,1481,1482,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1482,1482,1481,1483,1482,1483,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1483,1483,1482,1484,1483,1484,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1484,1484,1483,1485,1484,1485,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1485,1485,1484,1486,1485,1486,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1486,1486,1485,1487,1486,1487,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1487,1487,1486,1488,1487,1488,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1488,1488,1487,1489,1488,1489,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1489,1489,1488,1490,1489,1490,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1490,1490,1489,1491,1490,1491,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1491,1491,1490,1492,1491,1492,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1492,1492,1491,1493,1492,1493,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1493,1493,1492,1494,1493,1494,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1494,1494,1493,1495,1494,1495,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1495,1495,1494,1496,1495,1496,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1496,1496,1495,1497,1496,1497,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1497,1497,1496,1498,1497,1498,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1498,1498,1497,1499,1498,1499,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1499,1499,1498,1500,1499,1500,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1500,1500,1499,1501,1500,1501,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1501,1501,1500,1502,1501,1502,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1502,1502,1501,1503,1502,1503,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1503,1503,1502,1504,1503,1504,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1504,1504,1503,1505,1504,1505,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1505,1505,1504,1506,1505,1506,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1506,1506,1505,1507,1506,1507,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1507,1507,1506,1508,1507,1508,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1508,1508,1507,1509,1508,1509,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1509,1509,1508,1510,1509,1510,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1510,1510,1509,1511,1510,1511,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1511,1511,1510,1512,1511,1512,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1512,1512,1511,1513,1512,1513,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1513,1513,1512,1514,1513,1514,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1514,1514,1513,1515,1514,1515,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1515,1515,1514,1516,1515,1516,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1516,1516,1515,1517,1516,1517,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1517,1517,1516,1518,1517,1518,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1518,1518,1517,1519,1518,1519,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1519,1519,1518,1520,1519,1520,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1520,1520,1519,1521,1520,1521,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1521,1521,1520,1522,1521,1522,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1522,1522,1521,1523,1522,1523,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1523,1523,1522,1524,1523,1524,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1524,1524,1523,1525,1524,1525,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1525,1525,1524,1526,1525,1526,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1526,1526,1525,1527,1526,1527,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1527,1527,1526,1528,1527,1528,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1528,1528,1527,1529,1528,1529,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1529,1529,1528,1530,1529,1530,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1530,1530,1529,1531,1530,1531,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1531,1531,1530,1532,1531,1532,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1532,1532,1531,1533,1532,1533,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1533,1533,1532,1534,1533,1534,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1534,1534,1533,1535,1534,1535,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1535,1535,1534,1536,1535,1536,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1536,1536,1535,1537,1536,1537,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1537,1537,1536,0,1537,1538,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv +new file mode 100644 +index 00000000..6aac5521 +--- /dev/null ++++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/node.csv +@@ -0,0 +1,539 @@ ++NID,PID ++1001,1001 ++1002,1002 ++1003,1003 ++1004,1004 ++1005,1005 ++1006,1006 ++1007,1007 ++1008,1008 ++1009,1009 ++1010,1010 ++1011,1011 ++1012,1012 ++1013,1013 ++1014,1014 ++1015,1015 ++1016,1016 ++1017,1017 ++1018,1018 ++1019,1019 ++1020,1020 ++1021,1021 ++1022,1022 ++1023,1023 ++1024,1024 ++1025,1025 ++1026,1026 ++1027,1027 ++1028,1028 ++1029,1029 ++1030,1030 ++1031,1031 ++1032,1032 ++1033,1033 ++1034,1034 ++1035,1035 ++1036,1036 ++1037,1037 ++1038,1038 ++1039,1039 ++1040,1040 ++1041,1041 ++1042,1042 ++1043,1043 ++1044,1044 ++1045,1045 ++1046,1046 ++1047,1047 ++1048,1048 ++1049,1049 ++1050,1050 ++1051,1051 ++1052,1052 ++1053,1053 ++1054,1054 ++1055,1055 ++1056,1056 ++1057,1057 ++1058,1058 ++1059,1059 ++1060,1060 ++1061,1061 ++1062,1062 ++1063,1063 ++1064,1064 ++1065,1065 ++1066,1066 ++1067,1067 ++1068,1068 ++1069,1069 ++1070,1070 ++1071,1071 ++1072,1072 ++1073,1073 ++1074,1074 ++1075,1075 ++1076,1076 ++1077,1077 ++1078,1078 ++1079,1079 ++1080,1080 ++1081,1081 ++1082,1082 ++1083,1083 ++1084,1084 ++1085,1085 ++1086,1086 ++1087,1087 ++1088,1088 ++1089,1089 ++1090,1090 ++1091,1091 ++1092,1092 ++1093,1093 ++1094,1094 ++1095,1095 ++1096,1096 ++1097,1097 ++1098,1098 ++1099,1099 ++1100,1100 ++1101,1101 ++1102,1102 ++1103,1103 ++1104,1104 ++1105,1105 ++1106,1106 ++1107,1107 ++1108,1108 ++1109,1109 ++1110,1110 ++1111,1111 ++1112,1112 ++1113,1113 ++1114,1114 ++1115,1115 ++1116,1116 ++1117,1117 ++1118,1118 ++1119,1119 ++1120,1120 ++1121,1121 ++1122,1122 ++1123,1123 ++1124,1124 ++1125,1125 ++1126,1126 ++1127,1127 ++1128,1128 ++1129,1129 ++1130,1130 ++1131,1131 ++1132,1132 ++1133,1133 ++1134,1134 ++1135,1135 ++1136,1136 ++1137,1137 ++1138,1138 ++1139,1139 ++1140,1140 ++1141,1141 ++1142,1142 ++1143,1143 ++1144,1144 ++1145,1145 ++1146,1146 ++1147,1147 ++1148,1148 ++1149,1149 ++1150,1150 ++1151,1151 ++1152,1152 ++1153,1153 ++1154,1154 ++1155,1155 ++1156,1156 ++1157,1157 ++1158,1158 ++1159,1159 ++1160,1160 ++1161,1161 ++1162,1162 ++1163,1163 ++1164,1164 ++1165,1165 ++1166,1166 ++1167,1167 ++1168,1168 ++1169,1169 ++1170,1170 ++1171,1171 ++1172,1172 ++1173,1173 ++1174,1174 ++1175,1175 ++1176,1176 ++1177,1177 ++1178,1178 ++1179,1179 ++1180,1180 ++1181,1181 ++1182,1182 ++1183,1183 ++1184,1184 ++1185,1185 ++1186,1186 ++1187,1187 ++1188,1188 ++1189,1189 ++1190,1190 ++1191,1191 ++1192,1192 ++1193,1193 ++1194,1194 ++1195,1195 ++1196,1196 ++1197,1197 ++1198,1198 ++1199,1199 ++1200,1200 ++1201,1201 ++1202,1202 ++1203,1203 ++1204,1204 ++1205,1205 ++1206,1206 ++1207,1207 ++1208,1208 ++1209,1209 ++1210,1210 ++1211,1211 ++1212,1212 ++1213,1213 ++1214,1214 ++1215,1215 ++1216,1216 ++1217,1217 ++1218,1218 ++1219,1219 ++1220,1220 ++1221,1221 ++1222,1222 ++1223,1223 ++1224,1224 ++1225,1225 ++1226,1226 ++1227,1227 ++1228,1228 ++1229,1229 ++1230,1230 ++1231,1231 ++1232,1232 ++1233,1233 ++1234,1234 ++1235,1235 ++1236,1236 ++1237,1237 ++1238,1238 ++1239,1239 ++1240,1240 ++1241,1241 ++1242,1242 ++1243,1243 ++1244,1244 ++1245,1245 ++1246,1246 ++1247,1247 ++1248,1248 ++1249,1249 ++1250,1250 ++1251,1251 ++1252,1252 ++1253,1253 ++1254,1254 ++1255,1255 ++1256,1256 ++1257,1257 ++1258,1258 ++1259,1259 ++1260,1260 ++1261,1261 ++1262,1262 ++1263,1263 ++1264,1264 ++1265,1265 ++1266,1266 ++1267,1267 ++1268,1268 ++1269,1269 ++1270,1270 ++1271,1271 ++1272,1272 ++1273,1273 ++1274,1274 ++1275,1275 ++1276,1276 ++1277,1277 ++1278,1278 ++1279,1279 ++1280,1280 ++1281,1281 ++1282,1282 ++1283,1283 ++1284,1284 ++1285,1285 ++1286,1286 ++1287,1287 ++1288,1288 ++1289,1289 ++1290,1290 ++1291,1291 ++1292,1292 ++1293,1293 ++1294,1294 ++1295,1295 ++1296,1296 ++1297,1297 ++1298,1298 ++1299,1299 ++1300,1300 ++1301,1301 ++1302,1302 ++1303,1303 ++1304,1304 ++1305,1305 ++1306,1306 ++1307,1307 ++1308,1308 ++1309,1309 ++1310,1310 ++1311,1311 ++1312,1312 ++1313,1313 ++1314,1314 ++1315,1315 ++1316,1316 ++1317,1317 ++1318,1318 ++1319,1319 ++1320,1320 ++1321,1321 ++1322,1322 ++1323,1323 ++1324,1324 ++1325,1325 ++1326,1326 ++1327,1327 ++1328,1328 ++1329,1329 ++1330,1330 ++1331,1331 ++1332,1332 ++1333,1333 ++1334,1334 ++1335,1335 ++1336,1336 ++1337,1337 ++1338,1338 ++1339,1339 ++1340,1340 ++1341,1341 ++1342,1342 ++1343,1343 ++1344,1344 ++1345,1345 ++1346,1346 ++1347,1347 ++1348,1348 ++1349,1349 ++1350,1350 ++1351,1351 ++1352,1352 ++1353,1353 ++1354,1354 ++1355,1355 ++1356,1356 ++1357,1357 ++1358,1358 ++1359,1359 ++1360,1360 ++1361,1361 ++1362,1362 ++1363,1363 ++1364,1364 ++1365,1365 ++1366,1366 ++1367,1367 ++1368,1368 ++1369,1369 ++1370,1370 ++1371,1371 ++1372,1372 ++1373,1373 ++1374,1374 ++1375,1375 ++1376,1376 ++1377,1377 ++1378,1378 ++1379,1379 ++1380,1380 ++1381,1381 ++1382,1382 ++1383,1383 ++1384,1384 ++1385,1385 ++1386,1386 ++1387,1387 ++1388,1388 ++1389,1389 ++1390,1390 ++1391,1391 ++1392,1392 ++1393,1393 ++1394,1394 ++1395,1395 ++1396,1396 ++1397,1397 ++1398,1398 ++1399,1399 ++1400,1400 ++1401,1401 ++1402,1402 ++1403,1403 ++1404,1404 ++1405,1405 ++1406,1406 ++1407,1407 ++1408,1408 ++1409,1409 ++1410,1410 ++1411,1411 ++1412,1412 ++1413,1413 ++1414,1414 ++1415,1415 ++1416,1416 ++1417,1417 ++1418,1418 ++1419,1419 ++1420,1420 ++1421,1421 ++1422,1422 ++1423,1423 ++1424,1424 ++1425,1425 ++1426,1426 ++1427,1427 ++1428,1428 ++1429,1429 ++1430,1430 ++1431,1431 ++1432,1432 ++1433,1433 ++1434,1434 ++1435,1435 ++1436,1436 ++1437,1437 ++1438,1438 ++1439,1439 ++1440,1440 ++1441,1441 ++1442,1442 ++1443,1443 ++1444,1444 ++1445,1445 ++1446,1446 ++1447,1447 ++1448,1448 ++1449,1449 ++1450,1450 ++1451,1451 ++1452,1452 ++1453,1453 ++1454,1454 ++1455,1455 ++1456,1456 ++1457,1457 ++1458,1458 ++1459,1459 ++1460,1460 ++1461,1461 ++1462,1462 ++1463,1463 ++1464,1464 ++1465,1465 ++1466,1466 ++1467,1467 ++1468,1468 ++1469,1469 ++1470,1470 ++1471,1471 ++1472,1472 ++1473,1473 ++1474,1474 ++1475,1475 ++1476,1476 ++1477,1477 ++1478,1478 ++1479,1479 ++1480,1480 ++1481,1481 ++1482,1482 ++1483,1483 ++1484,1484 ++1485,1485 ++1486,1486 ++1487,1487 ++1488,1488 ++1489,1489 ++1490,1490 ++1491,1491 ++1492,1492 ++1493,1493 ++1494,1494 ++1495,1495 ++1496,1496 ++1497,1497 ++1498,1498 ++1499,1499 ++1500,1500 ++1501,1501 ++1502,1502 ++1503,1503 ++1504,1504 ++1505,1505 ++1506,1506 ++1507,1507 ++1508,1508 ++1509,1509 ++1510,1510 ++1511,1511 ++1512,1512 ++1513,1513 ++1514,1514 ++1515,1515 ++1516,1516 ++1517,1517 ++1518,1518 ++1519,1519 ++1520,1520 ++1521,1521 ++1522,1522 ++1523,1523 ++1524,1524 ++1525,1525 ++1526,1526 ++1527,1527 ++1528,1528 ++1529,1529 ++1530,1530 ++1531,1531 ++1532,1532 ++1533,1533 ++1534,1534 ++1535,1535 ++1536,1536 ++1537,1537 ++1538,1538 +diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv +new file mode 100644 +index 00000000..49c7cdc7 +--- /dev/null ++++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_circle_not_connected/point.csv +@@ -0,0 +1,539 @@ ++PID,B(Lat),L(Long),H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3 ++1001,0,0,0.041,129.675,314.065,0,0,0,0 ++1002,0,0,0.04,130.691,314.065,0,0,0,0 ++1003,0,0,0.04,131.708,314.064,0,0,0,0 ++1004,0,0,0.04,132.718,314.064,0,0,0,0 ++1005,0,0,0.04,133.745,314.064,0,0,0,0 ++1006,0,0,0.04,134.801,314.066,0,0,0,0 ++1007,0,0,0.04,135.862,314.068,0,0,0,0 ++1008,0,0,0.04,136.865,314.07,0,0,0,0 ++1009,0,0,0.04,137.897,314.072,0,0,0,0 ++1010,0,0,0.04,138.902,314.075,0,0,0,0 ++1011,0,0,0.04,139.921,314.077,0,0,0,0 ++1012,0,0,0.04,140.939,314.08,0,0,0,0 ++1013,0,0,0.04,141.951,314.082,0,0,0,0 ++1014,0,0,0.04,142.983,314.084,0,0,0,0 ++1015,0,0,0.04,144.003,314.086,0,0,0,0 ++1016,0,0,0.04,145.06,314.106,0,0,0,0 ++1017,0,0,0.04,146.06,314.118,0,0,0,0 ++1018,0,0,0.04,147.107,314.129,0,0,0,0 ++1019,0,0,0.04,148.109,314.14,0,0,0,0 ++1020,0,0,0.04,149.118,314.15,0,0,0,0 ++1021,0,0,0.04,150.136,314.161,0,0,0,0 ++1022,0,0,0.04,151.154,314.172,0,0,0,0 ++1023,0,0,0.04,152.171,314.183,0,0,0,0 ++1024,0,0,0.04,153.198,314.193,0,0,0,0 ++1025,0,0,0.04,154.224,314.204,0,0,0,0 ++1026,0,0,0.04,155.287,314.215,0,0,0,0 ++1027,0,0,0.04,156.302,314.226,0,0,0,0 ++1028,0,0,0.04,157.312,314.237,0,0,0,0 ++1029,0,0,0.04,158.379,314.248,0,0,0,0 ++1030,0,0,0.04,159.455,314.259,0,0,0,0 ++1031,0,0,0.04,160.51,314.271,0,0,0,0 ++1032,0,0,0.044,161.521,314.282,0,0,0,0 ++1033,0,0,0.057,162.574,314.296,0,0,0,0 ++1034,0,0,0.073,163.634,314.311,0,0,0,0 ++1035,0,0,0.081,164.666,314.326,0,0,0,0 ++1036,0,0,0.097,165.729,314.344,0,0,0,0 ++1037,0,0,0.107,166.753,314.363,0,0,0,0 ++1038,0,0,0.106,167.858,314.382,0,0,0,0 ++1039,0,0,0.106,168.978,314.402,0,0,0,0 ++1040,0,0,0.106,170.06,314.42,0,0,0,0 ++1041,0,0,0.106,171.126,314.438,0,0,0,0 ++1042,0,0,0.105,172.155,314.455,0,0,0,0 ++1043,0,0,0.105,173.164,314.473,0,0,0,0 ++1044,0,0,0.105,174.225,314.49,0,0,0,0 ++1045,0,0,0.099,175.296,314.508,0,0,0,0 ++1046,0,0,0.083,176.319,314.524,0,0,0,0 ++1047,0,0,0.076,177.46,314.542,0,0,0,0 ++1048,0,0,0.059,178.598,314.558,0,0,0,0 ++1049,0,0,0.047,179.602,314.571,0,0,0,0 ++1050,0,0,0.043,180.621,314.584,0,0,0,0 ++1051,0,0,0.04,181.718,314.598,0,0,0,0 ++1052,0,0,0.04,182.79,314.611,0,0,0,0 ++1053,0,0,0.04,183.825,314.624,0,0,0,0 ++1054,0,0,0.04,184.87,314.637,0,0,0,0 ++1055,0,0,0.04,185.896,314.649,0,0,0,0 ++1056,0,0,0.04,187.05,314.664,0,0,0,0 ++1057,0,0,0.04,188.124,314.677,0,0,0,0 ++1058,0,0,0.04,189.203,314.69,0,0,0,0 ++1059,0,0,0.04,190.271,314.703,0,0,0,0 ++1060,0,0,0.04,191.384,314.717,0,0,0,0 ++1061,0,0,0.04,192.396,314.729,0,0,0,0 ++1062,0,0,0.04,193.474,314.743,0,0,0,0 ++1063,0,0,0.04,194.501,314.756,0,0,0,0 ++1064,0,0,0.04,195.517,314.768,0,0,0,0 ++1065,0,0,0.04,196.577,314.781,0,0,0,0 ++1066,0,0,0.04,197.587,314.794,0,0,0,0 ++1067,0,0,0.04,198.625,314.806,0,0,0,0 ++1068,0,0,0.04,199.67,314.819,0,0,0,0 ++1069,0,0,0.04,200.715,314.832,0,0,0,0 ++1070,0,0,0.04,201.723,314.844,0,0,0,0 ++1071,0,0,0.04,202.797,314.858,0,0,0,0 ++1072,0,0,0.04,203.836,314.871,0,0,0,0 ++1073,0,0,0.04,204.877,314.884,0,0,0,0 ++1074,0,0,0.04,205.9,314.896,0,0,0,0 ++1075,0,0,0.04,206.914,314.908,0,0,0,0 ++1076,0,0,0.04,207.922,314.92,0,0,0,0 ++1077,0,0,0.04,208.936,314.931,0,0,0,0 ++1078,0,0,0.04,209.952,314.943,0,0,0,0 ++1079,0,0,0.04,210.974,314.955,0,0,0,0 ++1080,0,0,0.04,211.985,314.966,0,0,0,0 ++1081,0,0,0.04,213.011,314.978,0,0,0,0 ++1082,0,0,0.04,214.037,314.99,0,0,0,0 ++1083,0,0,0.04,215.109,315.002,0,0,0,0 ++1084,0,0,0.04,216.203,315.014,0,0,0,0 ++1085,0,0,0.04,217.212,315.026,0,0,0,0 ++1086,0,0,0.04,218.226,315.038,0,0,0,0 ++1087,0,0,0.04,219.321,315.05,0,0,0,0 ++1088,0,0,0.04,220.33,315.062,0,0,0,0 ++1089,0,0,0.04,221.382,315.074,0,0,0,0 ++1090,0,0,0.04,222.454,315.086,0,0,0,0 ++1091,0,0,0.04,223.615,315.099,0,0,0,0 ++1092,0,0,0.04,224.672,315.111,0,0,0,0 ++1093,0,0,0.04,225.705,315.123,0,0,0,0 ++1094,0,0,0.04,226.826,315.136,0,0,0,0 ++1095,0,0,0.04,227.872,315.148,0,0,0,0 ++1096,0,0,0.04,228.885,315.16,0,0,0,0 ++1097,0,0,0.04,230.0,315.173,0,0,0,0 ++1098,0,0,0.04,231.02,315.184,0,0,0,0 ++1099,0,0,0.04,232.043,315.189,0,0,0,0 ++1100,0,0,0.04,233.113,315.197,0,0,0,0 ++1101,0,0,0.04,234.143,315.205,0,0,0,0 ++1102,0,0,0.04,235.147,315.214,0,0,0,0 ++1103,0,0,0.04,236.19,315.223,0,0,0,0 ++1104,0,0,0.04,237.216,315.232,0,0,0,0 ++1105,0,0,0.04,238.337,315.243,0,0,0,0 ++1106,0,0,0.04,239.383,315.253,0,0,0,0 ++1107,0,0,0.04,240.532,315.263,0,0,0,0 ++1108,0,0,0.04,241.549,315.273,0,0,0,0 ++1109,0,0,0.04,242.63,315.283,0,0,0,0 ++1110,0,0,0.04,243.672,315.293,0,0,0,0 ++1111,0,0,0.04,244.725,315.303,0,0,0,0 ++1112,0,0,0.04,245.757,315.312,0,0,0,0 ++1113,0,0,0.04,246.832,315.322,0,0,0,0 ++1114,0,0,0.04,247.849,315.332,0,0,0,0 ++1115,0,0,0.04,248.861,315.341,0,0,0,0 ++1116,0,0,0.04,249.983,315.351,0,0,0,0 ++1117,0,0,0.04,251.03,315.36,0,0,0,0 ++1118,0,0,0.04,252.044,315.369,0,0,0,0 ++1119,0,0,0.04,253.082,315.379,0,0,0,0 ++1120,0,0,0.04,254.086,315.388,0,0,0,0 ++1121,0,0,0.04,255.094,315.396,0,0,0,0 ++1122,0,0,0.04,256.112,315.406,0,0,0,0 ++1123,0,0,0.04,257.132,315.415,0,0,0,0 ++1124,0,0,0.04,258.155,315.424,0,0,0,0 ++1125,0,0,0.04,259.207,315.434,0,0,0,0 ++1126,0,0,0.04,260.216,315.443,0,0,0,0 ++1127,0,0,0.04,261.256,315.451,0,0,0,0 ++1128,0,0,0.04,262.261,315.46,0,0,0,0 ++1129,0,0,0.04,263.277,315.468,0,0,0,0 ++1130,0,0,0.04,264.326,315.477,0,0,0,0 ++1131,0,0,0.04,265.345,315.486,0,0,0,0 ++1132,0,0,0.04,266.411,315.495,0,0,0,0 ++1133,0,0,0.04,267.451,315.489,0,0,0,0 ++1134,0,0,0.04,268.476,315.468,0,0,0,0 ++1135,0,0,0.04,269.535,315.413,0,0,0,0 ++1136,0,0,0.04,270.531,315.265,0,0,0,0 ++1137,0,0,0.04,271.547,315.158,0,0,0,0 ++1138,0,0,0.04,272.559,315.05,0,0,0,0 ++1139,0,0,0.04,273.578,314.942,0,0,0,0 ++1140,0,0,0.04,274.575,314.835,0,0,0,0 ++1141,0,0,0.04,275.579,314.715,0,0,0,0 ++1142,0,0,0.04,276.575,314.54,0,0,0,0 ++1143,0,0,0.04,277.614,314.381,0,0,0,0 ++1144,0,0,0.04,278.601,314.204,0,0,0,0 ++1145,0,0,0.04,279.581,313.96,0,0,0,0 ++1146,0,0,0.04,280.56,313.714,0,0,0,0 ++1147,0,0,0.04,281.543,313.481,0,0,0,0 ++1148,0,0,0.04,282.527,313.248,0,0,0,0 ++1149,0,0,0.04,283.519,313.013,0,0,0,0 ++1150,0,0,0.04,284.52,312.746,0,0,0,0 ++1151,0,0,0.04,285.475,312.445,0,0,0,0 ++1152,0,0,0.04,286.412,312.09,0,0,0,0 ++1153,0,0,0.04,287.373,311.745,0,0,0,0 ++1154,0,0,0.04,288.343,311.409,0,0,0,0 ++1155,0,0,0.04,289.274,311.0,0,0,0,0 ++1156,0,0,0.04,290.205,310.624,0,0,0,0 ++1157,0,0,0.04,291.159,310.24,0,0,0,0 ++1158,0,0,0.04,292.084,309.796,0,0,0,0 ++1159,0,0,0.04,293.016,309.367,0,0,0,0 ++1160,0,0,0.04,293.971,308.908,0,0,0,0 ++1161,0,0,0.04,294.88,308.483,0,0,0,0 ++1162,0,0,0.04,295.784,308.018,0,0,0,0 ++1163,0,0,0.04,296.743,307.478,0,0,0,0 ++1164,0,0,0.04,297.636,306.93,0,0,0,0 ++1165,0,0,0.04,298.498,306.347,0,0,0,0 ++1166,0,0,0.04,299.335,305.787,0,0,0,0 ++1167,0,0,0.04,300.13,305.165,0,0,0,0 ++1168,0,0,0.04,300.874,304.497,0,0,0,0 ++1169,0,0,0.04,301.67,303.856,0,0,0,0 ++1170,0,0,0.04,302.439,303.194,0,0,0,0 ++1171,0,0,0.04,303.161,302.486,0,0,0,0 ++1172,0,0,0.04,303.779,301.7,0,0,0,0 ++1173,0,0,0.04,304.384,300.861,0,0,0,0 ++1174,0,0,0.04,304.956,300.033,0,0,0,0 ++1175,0,0,0.04,305.533,299.183,0,0,0,0 ++1176,0,0,0.04,306.09,298.344,0,0,0,0 ++1177,0,0,0.04,306.609,297.475,0,0,0,0 ++1178,0,0,0.04,307.14,296.601,0,0,0,0 ++1179,0,0,0.04,307.616,295.704,0,0,0,0 ++1180,0,0,0.04,307.944,294.752,0,0,0,0 ++1181,0,0,0.04,308.317,293.808,0,0,0,0 ++1182,0,0,0.04,308.686,292.858,0,0,0,0 ++1183,0,0,0.04,308.984,291.883,0,0,0,0 ++1184,0,0,0.04,309.315,290.926,0,0,0,0 ++1185,0,0,0.04,309.589,289.947,0,0,0,0 ++1186,0,0,0.04,309.848,288.964,0,0,0,0 ++1187,0,0,0.04,309.999,287.97,0,0,0,0 ++1188,0,0,0.04,310.177,286.946,0,0,0,0 ++1189,0,0,0.04,310.283,285.926,0,0,0,0 ++1190,0,0,0.04,310.371,284.912,0,0,0,0 ++1191,0,0,0.04,310.447,283.908,0,0,0,0 ++1192,0,0,0.04,310.508,282.891,0,0,0,0 ++1193,0,0,0.04,310.559,281.88,0,0,0,0 ++1194,0,0,0.04,310.597,280.847,0,0,0,0 ++1195,0,0,0.04,310.647,279.829,0,0,0,0 ++1196,0,0,0.04,310.697,278.789,0,0,0,0 ++1197,0,0,0.04,310.745,277.778,0,0,0,0 ++1198,0,0,0.04,310.781,276.772,0,0,0,0 ++1199,0,0,0.04,310.795,275.754,0,0,0,0 ++1200,0,0,0.04,310.794,274.732,0,0,0,0 ++1201,0,0,0.04,310.806,273.729,0,0,0,0 ++1202,0,0,0.04,310.819,272.663,0,0,0,0 ++1203,0,0,0.04,310.831,271.638,0,0,0,0 ++1204,0,0,0.04,310.843,270.601,0,0,0,0 ++1205,0,0,0.04,310.854,269.569,0,0,0,0 ++1206,0,0,0.04,310.866,268.519,0,0,0,0 ++1207,0,0,0.04,310.878,267.47,0,0,0,0 ++1208,0,0,0.04,310.889,266.455,0,0,0,0 ++1209,0,0,0.04,310.901,265.416,0,0,0,0 ++1210,0,0,0.04,310.913,264.382,0,0,0,0 ++1211,0,0,0.04,310.924,263.362,0,0,0,0 ++1212,0,0,0.041,310.936,262.355,0,0,0,0 ++1213,0,0,0.041,310.948,261.253,0,0,0,0 ++1214,0,0,0.041,310.961,260.185,0,0,0,0 ++1215,0,0,0.042,310.973,259.12,0,0,0,0 ++1216,0,0,0.042,310.985,258.089,0,0,0,0 ++1217,0,0,0.042,310.997,257.076,0,0,0,0 ++1218,0,0,0.042,311.009,256.063,0,0,0,0 ++1219,0,0,0.042,311.022,255.038,0,0,0,0 ++1220,0,0,0.042,311.034,254.01,0,0,0,0 ++1221,0,0,0.041,311.046,252.982,0,0,0,0 ++1222,0,0,0.041,311.058,251.978,0,0,0,0 ++1223,0,0,0.04,311.07,250.92,0,0,0,0 ++1224,0,0,0.04,311.081,249.888,0,0,0,0 ++1225,0,0,0.04,311.091,248.882,0,0,0,0 ++1226,0,0,0.04,311.101,247.881,0,0,0,0 ++1227,0,0,0.04,311.11,246.835,0,0,0,0 ++1228,0,0,0.04,311.12,245.793,0,0,0,0 ++1229,0,0,0.04,311.129,244.762,0,0,0,0 ++1230,0,0,0.04,311.138,243.746,0,0,0,0 ++1231,0,0,0.04,311.147,242.739,0,0,0,0 ++1232,0,0,0.04,311.156,241.721,0,0,0,0 ++1233,0,0,0.04,311.164,240.714,0,0,0,0 ++1234,0,0,0.04,311.174,239.674,0,0,0,0 ++1235,0,0,0.04,311.184,238.671,0,0,0,0 ++1236,0,0,0.04,311.193,237.614,0,0,0,0 ++1237,0,0,0.04,311.202,236.601,0,0,0,0 ++1238,0,0,0.04,311.211,235.6,0,0,0,0 ++1239,0,0,0.04,311.221,234.531,0,0,0,0 ++1240,0,0,0.04,311.23,233.529,0,0,0,0 ++1241,0,0,0.04,311.239,232.499,0,0,0,0 ++1242,0,0,0.04,311.248,231.437,0,0,0,0 ++1243,0,0,0.04,311.257,230.436,0,0,0,0 ++1244,0,0,0.04,311.267,229.406,0,0,0,0 ++1245,0,0,0.04,311.276,228.387,0,0,0,0 ++1246,0,0,0.04,311.285,227.348,0,0,0,0 ++1247,0,0,0.04,311.294,226.335,0,0,0,0 ++1248,0,0,0.04,311.303,225.271,0,0,0,0 ++1249,0,0,0.04,311.311,224.217,0,0,0,0 ++1250,0,0,0.04,311.319,223.18,0,0,0,0 ++1251,0,0,0.04,311.327,222.147,0,0,0,0 ++1252,0,0,0.04,311.335,221.081,0,0,0,0 ++1253,0,0,0.04,311.342,220.077,0,0,0,0 ++1254,0,0,0.04,311.349,219.047,0,0,0,0 ++1255,0,0,0.04,311.355,218.017,0,0,0,0 ++1256,0,0,0.04,311.36,216.998,0,0,0,0 ++1257,0,0,0.04,311.365,215.981,0,0,0,0 ++1258,0,0,0.04,311.369,214.965,0,0,0,0 ++1259,0,0,0.04,311.373,213.941,0,0,0,0 ++1260,0,0,0.04,311.343,212.913,0,0,0,0 ++1261,0,0,0.042,311.248,211.906,0,0,0,0 ++1262,0,0,0.043,311.187,210.897,0,0,0,0 ++1263,0,0,0.044,311.046,209.899,0,0,0,0 ++1264,0,0,0.048,310.824,208.922,0,0,0,0 ++1265,0,0,0.05,310.516,207.949,0,0,0,0 ++1266,0,0,0.052,310.185,207.001,0,0,0,0 ++1267,0,0,0.055,309.748,206.095,0,0,0,0 ++1268,0,0,0.057,309.177,205.27,0,0,0,0 ++1269,0,0,0.059,308.593,204.446,0,0,0,0 ++1270,0,0,0.06,307.926,203.69,0,0,0,0 ++1271,0,0,0.06,307.133,203.067,0,0,0,0 ++1272,0,0,0.058,306.226,202.628,0,0,0,0 ++1273,0,0,0.057,305.273,202.298,0,0,0,0 ++1274,0,0,0.056,304.284,202.02,0,0,0,0 ++1275,0,0,0.056,303.281,201.874,0,0,0,0 ++1276,0,0,0.055,302.284,201.781,0,0,0,0 ++1277,0,0,0.055,301.277,201.779,0,0,0,0 ++1278,0,0,0.055,300.249,201.769,0,0,0,0 ++1279,0,0,0.055,299.234,201.716,0,0,0,0 ++1280,0,0,0.055,298.159,201.666,0,0,0,0 ++1281,0,0,0.055,297.145,201.589,0,0,0,0 ++1282,0,0,0.055,296.147,201.529,0,0,0,0 ++1283,0,0,0.055,295.121,201.467,0,0,0,0 ++1284,0,0,0.055,294.114,201.442,0,0,0,0 ++1285,0,0,0.055,293.11,201.403,0,0,0,0 ++1286,0,0,0.055,292.083,201.374,0,0,0,0 ++1287,0,0,0.055,291.079,201.34,0,0,0,0 ++1288,0,0,0.055,290.072,201.307,0,0,0,0 ++1289,0,0,0.055,289.048,201.274,0,0,0,0 ++1290,0,0,0.055,288.04,201.259,0,0,0,0 ++1291,0,0,0.055,287.022,201.236,0,0,0,0 ++1292,0,0,0.055,286.012,201.214,0,0,0,0 ++1293,0,0,0.055,285.001,201.192,0,0,0,0 ++1294,0,0,0.055,283.967,201.169,0,0,0,0 ++1295,0,0,0.055,282.912,201.145,0,0,0,0 ++1296,0,0,0.055,281.895,201.122,0,0,0,0 ++1297,0,0,0.055,280.838,201.097,0,0,0,0 ++1298,0,0,0.055,279.786,201.073,0,0,0,0 ++1299,0,0,0.055,278.779,201.05,0,0,0,0 ++1300,0,0,0.055,277.703,201.026,0,0,0,0 ++1301,0,0,0.055,276.592,201.001,0,0,0,0 ++1302,0,0,0.055,275.544,200.977,0,0,0,0 ++1303,0,0,0.055,274.516,200.954,0,0,0,0 ++1304,0,0,0.055,273.489,200.931,0,0,0,0 ++1305,0,0,0.055,272.469,200.908,0,0,0,0 ++1306,0,0,0.055,271.469,200.885,0,0,0,0 ++1307,0,0,0.055,270.465,200.862,0,0,0,0 ++1308,0,0,0.055,269.413,200.839,0,0,0,0 ++1309,0,0,0.055,268.395,200.816,0,0,0,0 ++1310,0,0,0.055,267.356,200.793,0,0,0,0 ++1311,0,0,0.055,266.344,200.77,0,0,0,0 ++1312,0,0,0.055,265.306,200.747,0,0,0,0 ++1313,0,0,0.055,264.206,200.722,0,0,0,0 ++1314,0,0,0.055,263.138,200.698,0,0,0,0 ++1315,0,0,0.055,262.101,200.675,0,0,0,0 ++1316,0,0,0.055,261.081,200.652,0,0,0,0 ++1317,0,0,0.055,259.992,200.628,0,0,0,0 ++1318,0,0,0.055,258.808,200.601,0,0,0,0 ++1319,0,0,0.055,257.639,200.575,0,0,0,0 ++1320,0,0,0.055,256.545,200.55,0,0,0,0 ++1321,0,0,0.055,255.51,200.527,0,0,0,0 ++1322,0,0,0.057,254.446,200.503,0,0,0,0 ++1323,0,0,0.057,253.43,200.48,0,0,0,0 ++1324,0,0,0.057,252.377,200.457,0,0,0,0 ++1325,0,0,0.058,251.305,200.433,0,0,0,0 ++1326,0,0,0.059,250.233,200.41,0,0,0,0 ++1327,0,0,0.06,249.195,200.387,0,0,0,0 ++1328,0,0,0.06,248.111,200.363,0,0,0,0 ++1329,0,0,0.06,247.042,200.333,0,0,0,0 ++1330,0,0,0.061,245.893,200.304,0,0,0,0 ++1331,0,0,0.06,244.748,200.275,0,0,0,0 ++1332,0,0,0.06,243.614,200.247,0,0,0,0 ++1333,0,0,0.059,242.603,200.221,0,0,0,0 ++1334,0,0,0.058,241.58,200.195,0,0,0,0 ++1335,0,0,0.057,240.545,200.168,0,0,0,0 ++1336,0,0,0.056,239.492,200.142,0,0,0,0 ++1337,0,0,0.055,238.416,200.114,0,0,0,0 ++1338,0,0,0.055,237.335,200.086,0,0,0,0 ++1339,0,0,0.055,236.265,200.059,0,0,0,0 ++1340,0,0,0.055,235.193,200.031,0,0,0,0 ++1341,0,0,0.055,234.171,200.007,0,0,0,0 ++1342,0,0,0.055,233.159,200.009,0,0,0,0 ++1343,0,0,0.055,232.016,199.995,0,0,0,0 ++1344,0,0,0.055,231.001,199.965,0,0,0,0 ++1345,0,0,0.055,229.934,199.938,0,0,0,0 ++1346,0,0,0.055,228.932,199.913,0,0,0,0 ++1347,0,0,0.055,227.91,199.888,0,0,0,0 ++1348,0,0,0.055,226.885,199.863,0,0,0,0 ++1349,0,0,0.055,225.858,199.838,0,0,0,0 ++1350,0,0,0.055,224.852,199.813,0,0,0,0 ++1351,0,0,0.055,223.798,199.787,0,0,0,0 ++1352,0,0,0.055,222.759,199.761,0,0,0,0 ++1353,0,0,0.055,221.686,199.734,0,0,0,0 ++1354,0,0,0.055,220.682,199.709,0,0,0,0 ++1355,0,0,0.055,219.634,199.684,0,0,0,0 ++1356,0,0,0.055,218.576,199.658,0,0,0,0 ++1357,0,0,0.055,217.558,199.632,0,0,0,0 ++1358,0,0,0.055,216.538,199.607,0,0,0,0 ++1359,0,0,0.055,215.439,199.58,0,0,0,0 ++1360,0,0,0.055,214.308,199.552,0,0,0,0 ++1361,0,0,0.055,213.153,199.524,0,0,0,0 ++1362,0,0,0.055,212.125,199.498,0,0,0,0 ++1363,0,0,0.055,211.068,199.472,0,0,0,0 ++1364,0,0,0.055,210.007,199.446,0,0,0,0 ++1365,0,0,0.055,208.957,199.42,0,0,0,0 ++1366,0,0,0.055,207.931,199.395,0,0,0,0 ++1367,0,0,0.055,206.88,199.369,0,0,0,0 ++1368,0,0,0.055,205.839,199.343,0,0,0,0 ++1369,0,0,0.055,204.773,199.317,0,0,0,0 ++1370,0,0,0.055,203.682,199.29,0,0,0,0 ++1371,0,0,0.055,202.545,199.261,0,0,0,0 ++1372,0,0,0.055,201.505,199.236,0,0,0,0 ++1373,0,0,0.055,200.451,199.21,0,0,0,0 ++1374,0,0,0.055,199.332,199.182,0,0,0,0 ++1375,0,0,0.055,198.283,199.156,0,0,0,0 ++1376,0,0,0.055,197.186,199.129,0,0,0,0 ++1377,0,0,0.055,196.108,199.102,0,0,0,0 ++1378,0,0,0.055,195.103,199.077,0,0,0,0 ++1379,0,0,0.055,194.052,199.051,0,0,0,0 ++1380,0,0,0.055,193.036,199.026,0,0,0,0 ++1381,0,0,0.055,191.994,199.001,0,0,0,0 ++1382,0,0,0.055,190.904,198.974,0,0,0,0 ++1383,0,0,0.055,189.806,198.946,0,0,0,0 ++1384,0,0,0.055,188.77,198.921,0,0,0,0 ++1385,0,0,0.055,187.678,198.894,0,0,0,0 ++1386,0,0,0.055,186.563,198.866,0,0,0,0 ++1387,0,0,0.055,185.384,198.837,0,0,0,0 ++1388,0,0,0.055,184.288,198.81,0,0,0,0 ++1389,0,0,0.055,183.269,198.785,0,0,0,0 ++1390,0,0,0.055,182.198,198.758,0,0,0,0 ++1391,0,0,0.055,181.154,198.733,0,0,0,0 ++1392,0,0,0.055,180.045,198.705,0,0,0,0 ++1393,0,0,0.061,179.011,198.68,0,0,0,0 ++1394,0,0,0.084,177.98,198.654,0,0,0,0 ++1395,0,0,0.081,176.959,198.63,0,0,0,0 ++1396,0,0,0.09,175.942,198.605,0,0,0,0 ++1397,0,0,0.11,174.916,198.582,0,0,0,0 ++1398,0,0,0.105,173.805,198.555,0,0,0,0 ++1399,0,0,0.103,172.759,198.531,0,0,0,0 ++1400,0,0,0.1,171.697,198.506,0,0,0,0 ++1401,0,0,0.098,170.696,198.482,0,0,0,0 ++1402,0,0,0.09,169.636,198.456,0,0,0,0 ++1403,0,0,0.082,168.595,198.43,0,0,0,0 ++1404,0,0,0.075,167.576,198.405,0,0,0,0 ++1405,0,0,0.066,166.532,198.391,0,0,0,0 ++1406,0,0,0.063,165.489,198.374,0,0,0,0 ++1407,0,0,0.059,164.481,198.356,0,0,0,0 ++1408,0,0,0.055,163.456,198.4,0,0,0,0 ++1409,0,0,0.055,162.455,198.412,0,0,0,0 ++1410,0,0,0.055,161.443,198.46,0,0,0,0 ++1411,0,0,0.055,160.439,198.491,0,0,0,0 ++1412,0,0,0.055,159.431,198.523,0,0,0,0 ++1413,0,0,0.055,158.375,198.558,0,0,0,0 ++1414,0,0,0.055,157.338,198.664,0,0,0,0 ++1415,0,0,0.055,156.273,198.732,0,0,0,0 ++1416,0,0,0.055,155.231,198.843,0,0,0,0 ++1417,0,0,0.055,154.229,198.947,0,0,0,0 ++1418,0,0,0.055,153.232,199.058,0,0,0,0 ++1419,0,0,0.055,152.225,199.232,0,0,0,0 ++1420,0,0,0.055,151.238,199.404,0,0,0,0 ++1421,0,0,0.055,150.264,199.651,0,0,0,0 ++1422,0,0,0.055,149.296,199.938,0,0,0,0 ++1423,0,0,0.055,148.332,200.217,0,0,0,0 ++1424,0,0,0.055,147.376,200.528,0,0,0,0 ++1425,0,0,0.055,146.404,200.869,0,0,0,0 ++1426,0,0,0.055,145.442,201.226,0,0,0,0 ++1427,0,0,0.055,144.477,201.566,0,0,0,0 ++1428,0,0,0.055,143.488,201.937,0,0,0,0 ++1429,0,0,0.055,142.551,202.319,0,0,0,0 ++1430,0,0,0.055,141.609,202.686,0,0,0,0 ++1431,0,0,0.055,140.712,203.159,0,0,0,0 ++1432,0,0,0.055,139.799,203.594,0,0,0,0 ++1433,0,0,0.055,138.893,204.081,0,0,0,0 ++1434,0,0,0.055,137.998,204.569,0,0,0,0 ++1435,0,0,0.055,137.096,205.05,0,0,0,0 ++1436,0,0,0.055,136.24,205.594,0,0,0,0 ++1437,0,0,0.055,135.408,206.148,0,0,0,0 ++1438,0,0,0.055,134.558,206.705,0,0,0,0 ++1439,0,0,0.055,133.735,207.296,0,0,0,0 ++1440,0,0,0.055,132.948,207.925,0,0,0,0 ++1441,0,0,0.055,132.133,208.534,0,0,0,0 ++1442,0,0,0.055,131.312,209.148,0,0,0,0 ++1443,0,0,0.055,130.509,209.833,0,0,0,0 ++1444,0,0,0.055,129.781,210.533,0,0,0,0 ++1445,0,0,0.055,129.016,211.238,0,0,0,0 ++1446,0,0,0.055,128.248,211.933,0,0,0,0 ++1447,0,0,0.055,127.482,212.625,0,0,0,0 ++1448,0,0,0.055,126.806,213.379,0,0,0,0 ++1449,0,0,0.055,126.041,214.151,0,0,0,0 ++1450,0,0,0.055,125.374,214.961,0,0,0,0 ++1451,0,0,0.055,124.817,215.813,0,0,0,0 ++1452,0,0,0.055,124.342,216.698,0,0,0,0 ++1453,0,0,0.055,123.913,217.627,0,0,0,0 ++1454,0,0,0.055,123.429,218.518,0,0,0,0 ++1455,0,0,0.055,123.013,219.457,0,0,0,0 ++1456,0,0,0.055,122.572,220.373,0,0,0,0 ++1457,0,0,0.055,122.12,221.308,0,0,0,0 ++1458,0,0,0.055,121.754,222.24,0,0,0,0 ++1459,0,0,0.055,121.354,223.191,0,0,0,0 ++1460,0,0,0.055,120.995,224.137,0,0,0,0 ++1461,0,0,0.055,120.681,225.09,0,0,0,0 ++1462,0,0,0.055,120.406,226.097,0,0,0,0 ++1463,0,0,0.055,120.176,227.112,0,0,0,0 ++1464,0,0,0.055,120.014,228.123,0,0,0,0 ++1465,0,0,0.055,119.842,229.165,0,0,0,0 ++1466,0,0,0.055,119.679,230.222,0,0,0,0 ++1467,0,0,0.055,119.573,231.253,0,0,0,0 ++1468,0,0,0.055,119.518,232.302,0,0,0,0 ++1469,0,0,0.055,119.498,233.305,0,0,0,0 ++1470,0,0,0.055,119.405,234.333,0,0,0,0 ++1471,0,0,0.055,119.316,235.356,0,0,0,0 ++1472,0,0,0.055,119.239,236.385,0,0,0,0 ++1473,0,0,0.055,119.177,237.395,0,0,0,0 ++1474,0,0,0.055,119.15,238.4,0,0,0,0 ++1475,0,0,0.055,119.103,239.416,0,0,0,0 ++1476,0,0,0.055,119.054,240.444,0,0,0,0 ++1477,0,0,0.055,119.007,241.464,0,0,0,0 ++1478,0,0,0.055,118.96,242.486,0,0,0,0 ++1479,0,0,0.055,118.92,243.524,0,0,0,0 ++1480,0,0,0.055,118.907,244.542,0,0,0,0 ++1481,0,0,0.055,118.879,245.583,0,0,0,0 ++1482,0,0,0.055,118.852,246.618,0,0,0,0 ++1483,0,0,0.055,118.837,247.64,0,0,0,0 ++1484,0,0,0.055,118.823,248.651,0,0,0,0 ++1485,0,0,0.055,118.807,249.659,0,0,0,0 ++1486,0,0,0.055,118.821,250.691,0,0,0,0 ++1487,0,0,0.055,118.822,251.724,0,0,0,0 ++1488,0,0,0.055,118.823,252.77,0,0,0,0 ++1489,0,0,0.055,118.825,253.772,0,0,0,0 ++1490,0,0,0.055,118.827,254.798,0,0,0,0 ++1491,0,0,0.055,118.829,255.804,0,0,0,0 ++1492,0,0,0.055,118.83,256.839,0,0,0,0 ++1493,0,0,0.055,118.832,257.845,0,0,0,0 ++1494,0,0,0.055,118.823,258.898,0,0,0,0 ++1495,0,0,0.055,118.817,259.937,0,0,0,0 ++1496,0,0,0.055,118.812,260.966,0,0,0,0 ++1497,0,0,0.055,118.806,261.979,0,0,0,0 ++1498,0,0,0.055,118.801,262.987,0,0,0,0 ++1499,0,0,0.055,118.795,263.994,0,0,0,0 ++1500,0,0,0.055,118.789,265.071,0,0,0,0 ++1501,0,0,0.055,118.771,266.232,0,0,0,0 ++1502,0,0,0.055,118.756,267.27,0,0,0,0 ++1503,0,0,0.055,118.741,268.31,0,0,0,0 ++1504,0,0,0.055,118.726,269.376,0,0,0,0 ++1505,0,0,0.055,118.711,270.427,0,0,0,0 ++1506,0,0,0.055,118.697,271.486,0,0,0,0 ++1507,0,0,0.055,118.682,272.544,0,0,0,0 ++1508,0,0,0.055,118.65,273.584,0,0,0,0 ++1509,0,0,0.055,118.624,274.625,0,0,0,0 ++1510,0,0,0.055,118.57,275.636,0,0,0,0 ++1511,0,0,0.055,118.526,276.675,0,0,0,0 ++1512,0,0,0.055,118.482,277.694,0,0,0,0 ++1513,0,0,0.055,118.437,278.698,0,0,0,0 ++1514,0,0,0.055,118.393,279.707,0,0,0,0 ++1515,0,0,0.055,118.347,280.73,0,0,0,0 ++1516,0,0,0.055,118.303,281.738,0,0,0,0 ++1517,0,0,0.055,118.257,282.757,0,0,0,0 ++1518,0,0,0.055,118.239,283.759,0,0,0,0 ++1519,0,0,0.055,118.21,284.764,0,0,0,0 ++1520,0,0,0.055,118.221,285.79,0,0,0,0 ++1521,0,0,0.055,118.21,286.829,0,0,0,0 ++1522,0,0,0.055,118.199,287.845,0,0,0,0 ++1523,0,0,0.055,118.189,288.869,0,0,0,0 ++1524,0,0,0.055,118.178,289.948,0,0,0,0 ++1525,0,0,0.055,118.167,290.972,0,0,0,0 ++1526,0,0,0.055,118.156,292.054,0,0,0,0 ++1527,0,0,0.055,118.145,293.133,0,0,0,0 ++1528,0,0,0.055,118.126,294.186,0,0,0,0 ++1529,0,0,0.055,118.099,295.187,0,0,0,0 ++1530,0,0,0.055,118.078,296.198,0,0,0,0 ++1531,0,0,0.055,118.056,297.239,0,0,0,0 ++1532,0,0,0.055,118.035,298.264,0,0,0,0 ++1533,0,0,0.055,118.014,299.268,0,0,0,0 ++1534,0,0,0.055,117.991,300.327,0,0,0,0 ++1535,0,0,0.055,117.971,301.328,0,0,0,0 ++1536,0,0,0.055,117.948,302.415,0,0,0,0 ++1537,0,0,0.055,117.927,303.433,0,0,0,0 ++1538,0,0,0.055,117.907,304.449,0,0,0,0 +diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv +new file mode 100644 +index 00000000..bdbe3cb0 +--- /dev/null ++++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/dtlane.csv +@@ -0,0 +1,160 @@ ++DID,Dist,PID,Dir,Apara,r,slope,cant,LW,RW ++1001,0,1001,1.5769552072461928,0,90000000000,0,0,2,2 ++1002,1,1002,1.5769552072461928,0,90000000000,0,0,2,2 ++1003,2,1003,1.5737316285471363,0,-310.21423497205427,0,0,2,2 ++1004,3,1004,1.5725114416586194,0,-819.5465870113417,0,0,2,2 ++1005,4,1005,1.57844374331971,0,168.5686361094724,0,0,2,2 ++1006,5,1006,1.5663694826442065,0,-82.8208059172365,0,0,2,2 ++1007,6,1007,1.5733395387307152,0,143.47086846770284,0,0,2,2 ++1008,7,1008,1.5792896863938737,0,168.06305601316365,0,0,2,2 ++1009,8,1009,1.5777890561648253,0,-666.3866825035068,0,0,2,2 ++1010,9,1010,1.5750140106955042,0,-360.3544558297345,0,0,2,2 ++1011,10,1011,1.5746689519147377,0,-2898.0569564947527,0,0,2,2 ++1012,11,1012,1.5687257363219358,0,-168.259082038207,0,0,2,2 ++1013,12,1013,1.5777541674329927,0,110.76121506596262,0,0,2,2 ++1014,13,1014,1.5729688000176911,0,-208.97037013342555,0,0,2,2 ++1015,14,1015,1.5804808525982976,0,133.11940901234524,0,0,2,2 ++1016,15,1016,1.5711309651106926,0,-106.95315866909446,0,0,2,2 ++1017,16,1017,1.5737926208556858,0,375.7059874783122,0,0,2,2 ++1018,17,1018,1.5761274035452846,0,428.30538553113007,0,0,2,2 ++1019,18,1019,1.5702416354892954,0,-169.90136044902954,0,0,2,2 ++1020,19,1020,1.578198355029532,0,125.67993567488075,0,0,2,2 ++1021,20,1021,1.5720181705458571,0,-161.80746750222914,0,0,2,2 ++1022,21,1022,1.5706055651130575,0,-707.9117613317552,0,0,2,2 ++1023,22,1023,1.568668187450491,0,-516.1616236843148,0,0,2,2 ++1024,23,1024,1.5738116929901291,0,194.4199325330892,0,0,2,2 ++1025,24,1025,1.5681610697806125,0,-176.97163001699232,0,0,2,2 ++1026,25,1026,1.576233551996162,0,123.87763432587761,0,0,2,2 ++1027,26,1027,1.5771205911255413,0,1127.3459838236336,0,0,2,2 ++1028,27,1028,1.572915733582594,0,-237.82018529433887,0,0,2,2 ++1029,28,1029,1.5762353397531932,0,301.2405534297222,0,0,2,2 ++1030,29,1030,1.5753996131209225,0,-1196.5635189618679,0,0,2,2 ++1031,30,1031,1.583323885143584,0,126.19455732214087,0,0,2,2 ++1032,31,1032,1.5763200772449486,0,-142.77947289143017,0,0,2,2 ++1033,32,1033,1.581582891357367,0,190.01241135238598,0,0,2,2 ++1034,33,1034,1.587602910539133,0,166.11242751998466,0,0,2,2 ++1035,34,1035,1.567492964011488,0,-49.7266364495457,0,0,2,2 ++1036,35,1036,1.580190068162111,0,78.75811587722826,0,0,2,2 ++1037,36,1037,1.5753588285894533,0,-206.98621646905497,0,0,2,2 ++1038,37,1038,1.5770779342184096,0,581.6978219116953,0,0,2,2 ++1039,38,1039,1.5778070212815072,0,1371.578307467697,0,0,2,2 ++1040,39,1040,1.57688054066635,0,-1079.3533978369474,0,0,2,2 ++1041,40,1041,1.5693145277312603,0,-132.17000930069833,0,0,2,2 ++1042,41,1042,1.5765282755232228,0,138.62419769016512,0,0,2,2 ++1043,42,1043,1.5620408548588791,0,-69.02539956344258,0,0,2,2 ++1044,43,1044,1.574392985650641,0,80.95769198517125,0,0,2,2 ++1045,44,1045,1.5781552304956135,0,265.7987561166538,0,0,2,2 ++1046,45,1046,1.5636488564992264,0,-68.9352142891846,0,0,2,2 ++1047,46,1047,1.576282511813594,0,79.15365546365315,0,0,2,2 ++1048,47,1048,1.5706338534883697,0,-177.0331895513095,0,0,2,2 ++1049,48,1049,1.5697332764760084,0,-1110.3992065908515,0,0,2,2 ++1050,49,1050,1.572074578284328,0,427.1128123877896,0,0,2,2 ++1051,50,1051,1.5709788389795292,0,-912.6258368395728,0,0,2,2 ++1052,51,1052,1.5707963267,0,-5479.083394168603,0,0,2,2 ++1053,52,1053,1.5750698586722753,0,233.99848333592325,0,0,2,2 ++1054,53,1054,1.5720016133095074,0,-325.9191758699087,0,0,2,2 ++1055,54,1055,1.5710102507774575,0,-1008.7127238228784,0,0,2,2 ++1056,55,1056,1.5741634461732454,0,317.1386084528101,0,0,2,2 ++1057,56,1057,1.5697313502942065,0,-225.6268878860186,0,0,2,2 ++1058,57,1058,1.5783787159974525,0,115.64215442219911,0,0,2,2 ++1059,58,1059,1.5701001710798361,0,-120.7941745743319,0,0,2,2 ++1060,59,1060,1.5760334581575526,0,168.54063976706658,0,0,2,2 ++1061,60,1061,1.5701780200451096,0,-170.78141392613784,0,0,2,2 ++1062,61,1062,1.5739481064805068,0,265.2459080542718,0,0,2,2 ++1063,62,1063,1.5760566970430587,0,474.25043901824483,0,0,2,2 ++1064,63,1064,1.5740497522036763,0,-498.2697981413879,0,0,2,2 ++1065,64,1065,1.5734512687362876,0,-1670.8899317858372,0,0,2,2 ++1066,65,1066,1.5766194230917199,0,315.641186574559,0,0,2,2 ++1067,66,1067,1.5694254082809356,0,-139.00443998265538,0,0,2,2 ++1068,67,1068,1.5794433316084266,0,99.82108739600862,0,0,2,2 ++1069,68,1069,1.5725889467252774,0,-145.8920117629209,0,0,2,2 ++1070,69,1070,1.5807873561845662,0,121.97487878176102,0,0,2,2 ++1071,70,1071,1.5739130500618594,0,-145.46922731544663,0,0,2,2 ++1072,71,1072,1.5718091127823781,0,-475.29933983893517,0,0,2,2 ++1073,72,1073,1.5707963267,0,-987.3753375954086,0,0,2,2 ++1074,73,1074,1.5787771988715407,0,125.29958862966704,0,0,2,2 ++1075,74,1075,1.5676375831346219,0,-89.76970333777409,0,0,2,2 ++1076,75,1076,1.5799596823472497,0,81.15500311628622,0,0,2,2 ++1077,76,1077,1.5718318856079825,0,-123.03457284663308,0,0,2,2 ++1078,77,1078,1.573141750227899,0,763.4376750046916,0,0,2,2 ++1079,78,1079,1.5728167843087149,0,-3077.245769373193,0,0,2,2 ++1080,79,1080,1.5785502637358915,0,174.41416031947614,0,0,2,2 ++1081,80,1081,1.569674339102679,0,-112.66431851597149,0,0,2,2 ++1082,81,1082,1.5762914026465802,0,151.12443659721072,0,0,2,2 ++1083,82,1083,1.5779954635538054,0,586.8334845075173,0,0,2,2 ++1084,83,1084,1.5718615508136478,0,-163.02807724230902,0,0,2,2 ++1085,84,1085,1.5801819377192137,0,120.18671864057802,0,0,2,2 ++1086,85,1086,1.5735396017275092,0,-150.54944544342285,0,0,2,2 ++1087,86,1087,1.5747067556354672,0,856.7850333890341,0,0,2,2 ++1088,87,1088,1.5794144265235939,0,212.4192671416631,0,0,2,2 ++1089,88,1089,1.5679123444827223,0,-86.94078136867701,0,0,2,2 ++1090,89,1090,1.5776885650677432,0,102.28901765291539,0,0,2,2 ++1091,90,1091,1.5772802126750645,0,-2448.8652887277844,0,0,2,2 ++1092,91,1092,1.5627397146614799,0,-68.77343534353095,0,0,2,2 ++1093,92,1093,1.585936009663618,0,43.110332917727945,0,0,2,2 ++1094,93,1094,1.5619935809487495,0,-41.7668571517556,0,0,2,2 ++1095,94,1095,1.5876906095116257,0,38.91500519420658,0,0,2,2 ++1096,95,1096,1.5454624617424062,0,-23.680887105564942,0,0,2,2 ++1097,96,1097,1.5873645175601516,0,23.86517750703063,0,0,2,2 ++1098,97,1098,1.570944021915078,0,-60.89950155067454,0,0,2,2 ++1099,98,1099,1.5807948187074246,0,101.51463085472905,0,0,2,2 ++1100,99,1100,1.571359730727884,0,-105.9873529709988,0,0,2,2 ++1101,100,1101,1.5698360710217596,0,-656.3145274371365,0,0,2,2 ++1102,101,1102,1.5778075823117197,0,125.4467269286155,0,0,2,2 ++1103,102,1103,1.5842673129254203,0,154.8052170904881,0,0,2,2 ++1104,103,1104,1.577986173407375,0,-159.20678041413706,0,0,2,2 ++1105,104,1105,1.5728062349336525,0,-193.0524860619341,0,0,2,2 ++1106,105,1106,1.5681396089673467,0,-214.28758319613829,0,0,2,2 ++1107,106,1107,1.5668894971276457,0,-799.9284289949252,0,0,2,2 ++1108,107,1108,1.576207785892031,0,107.31584149034198,0,0,2,2 ++1109,108,1109,1.5583652788716873,0,-56.04593563336265,0,0,2,2 ++1110,109,1110,1.577050748676428,0,53.51751978675366,0,0,2,2 ++1111,110,1111,1.5845470828584913,0,133.39853529912384,0,0,2,2 ++1112,111,1112,1.574603990145371,0,-100.57232984265129,0,0,2,2 ++1113,112,1113,1.5714870138239672,0,-320.8237397034875,0,0,2,2 ++1114,113,1114,1.5780119468725389,0,153.25827752652526,0,0,2,2 ++1115,114,1115,1.5843865999601219,0,156.87128166203587,0,0,2,2 ++1116,115,1116,1.5557032253732601,0,-34.863401339744854,0,0,2,2 ++1117,116,1117,1.576884531537484,0,47.211441647968,0,0,2,2 ++1118,117,1118,1.5766306482410994,0,-3938.817615182425,0,0,2,2 ++1119,118,1119,1.5734950747595073,0,-318.9209265452281,0,0,2,2 ++1120,119,1120,1.5775238109552248,0,248.21679837538343,0,0,2,2 ++1121,120,1121,1.5735982325932878,0,-254.73953333759928,0,0,2,2 ++1122,121,1122,1.5701436241046975,0,-289.46840236824585,0,0,2,2 ++1123,122,1123,1.5758215401952662,0,176.1209542460578,0,0,2,2 ++1124,123,1124,1.558982377070755,0,-59.385374000230954,0,0,2,2 ++1125,124,1125,1.5763367431657738,0,57.62238704224561,0,0,2,2 ++1126,125,1126,1.564720140088975,0,-86.0836849971439,0,0,2,2 ++1127,126,1127,1.576122881314287,0,87.69821047768663,0,0,2,2 ++1128,127,1128,1.579527990768055,0,293.6763160119224,0,0,2,2 ++1129,128,1129,1.566197129455978,0,-75.01390769807594,0,0,2,2 ++1130,129,1130,1.5793387649752906,0,76.09402943267055,0,0,2,2 ++1131,130,1131,1.5738126512839616,0,-180.95899864838228,0,0,2,2 ++1132,131,1132,1.5740190032649894,0,4846.088683128463,0,0,2,2 ++1133,132,1133,1.5713930387454167,0,-380.8124567359731,0,0,2,2 ++1134,133,1134,1.578585527369067,0,139.0339355854949,0,0,2,2 ++1135,134,1135,1.5764902367863745,0,-477.2607714940234,0,0,2,2 ++1136,135,1136,1.573288419766056,0,-312.32265730804284,0,0,2,2 ++1137,136,1137,1.579542170941775,0,159.90402750314496,0,0,2,2 ++1138,137,1138,1.573978822436062,0,-179.74786209656133,0,0,2,2 ++1139,138,1139,1.5810142195432424,0,142.13838746634403,0,0,2,2 ++1140,139,1140,1.5716636063774267,0,-106.94485829612124,0,0,2,2 ++1141,140,1141,1.5687473345739384,0,-342.9035657114844,0,0,2,2 ++1142,141,1142,1.5755972265830727,0,145.98770296911493,0,0,2,2 ++1143,142,1143,1.576111637781805,0,1943.9701205268652,0,0,2,2 ++1144,143,1144,1.5846480915453838,0,117.14466307620009,0,0,2,2 ++1145,144,1145,1.5696541500161478,0,-66.69360408336591,0,0,2,2 ++1146,145,1146,1.5665878699312612,0,-326.12806798990545,0,0,2,2 ++1147,146,1147,1.5736245608080908,0,142.11225382840146,0,0,2,2 ++1148,147,1148,1.5818404302554914,0,121.71566337588133,0,0,2,2 ++1149,148,1149,1.5635649606831077,0,-54.71815627167872,0,0,2,2 ++1150,149,1150,1.564744372765147,0,847.8800711205324,0,0,2,2 ++1151,150,1151,1.573462502288806,0,114.70350346209469,0,0,2,2 ++1152,151,1152,1.578323109330659,0,205.73561931449584,0,0,2,2 ++1153,152,1153,1.5844978766933193,0,161.94942113076553,0,0,2,2 ++1154,153,1154,1.5712725171401973,0,-75.61231102892319,0,0,2,2 ++1155,154,1155,1.5788883820139301,0,131.3048506741517,0,0,2,2 ++1156,155,1156,1.572986141831219,0,-169.42719527565234,0,0,2,2 ++1157,156,1157,1.5740141579748792,0,972.7473699387008,0,0,2,2 ++1158,157,1158,1.568363598139526,0,-176.97361485200645,0,0,2,2 ++1159,158,1159,1.5809705392166384,0,79.32138287022534,0,0,2,2 +diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv +new file mode 100644 +index 00000000..88652956 +--- /dev/null ++++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/lane.csv +@@ -0,0 +1,159 @@ ++LnID,DID,BLID,FLID,BNID,FNID,JCT,BLID2,BLID3,BLID4,FLID2,FLID3,FLID4,CrossID,Span,LCnt,Lno,LaneType,LimitVel,RefVel,RoadSecID,LaneChgFG ++1001,1001,0,1002,1001,1002,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1002,1002,1001,1003,1002,1003,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1003,1003,1002,1004,1003,1004,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1004,1004,1003,1005,1004,1005,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1005,1005,1004,1006,1005,1006,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1006,1006,1005,1007,1006,1007,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1007,1007,1006,1008,1007,1008,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1008,1008,1007,1009,1008,1009,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1009,1009,1008,1010,1009,1010,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1010,1010,1009,1011,1010,1011,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1011,1011,1010,1012,1011,1012,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1012,1012,1011,1013,1012,1013,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1013,1013,1012,1014,1013,1014,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1014,1014,1013,1015,1014,1015,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1015,1015,1014,1016,1015,1016,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1016,1016,1015,1017,1016,1017,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1017,1017,1016,1018,1017,1018,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1018,1018,1017,1019,1018,1019,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1019,1019,1018,1020,1019,1020,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1020,1020,1019,1021,1020,1021,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1021,1021,1020,1022,1021,1022,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1022,1022,1021,1023,1022,1023,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1023,1023,1022,1024,1023,1024,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1024,1024,1023,1025,1024,1025,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1025,1025,1024,1026,1025,1026,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1026,1026,1025,1027,1026,1027,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1027,1027,1026,1028,1027,1028,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1028,1028,1027,1029,1028,1029,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1029,1029,1028,1030,1029,1030,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1030,1030,1029,1031,1030,1031,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1031,1031,1030,1032,1031,1032,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1032,1032,1031,1033,1032,1033,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1033,1033,1032,1034,1033,1034,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1034,1034,1033,1035,1034,1035,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1035,1035,1034,1036,1035,1036,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1036,1036,1035,1037,1036,1037,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1037,1037,1036,1038,1037,1038,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1038,1038,1037,1039,1038,1039,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1039,1039,1038,1040,1039,1040,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1040,1040,1039,1041,1040,1041,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1041,1041,1040,1042,1041,1042,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1042,1042,1041,1043,1042,1043,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1043,1043,1042,1044,1043,1044,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1044,1044,1043,1045,1044,1045,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1045,1045,1044,1046,1045,1046,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1046,1046,1045,1047,1046,1047,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1047,1047,1046,1048,1047,1048,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1048,1048,1047,1049,1048,1049,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1049,1049,1048,1050,1049,1050,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1050,1050,1049,1051,1050,1051,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1051,1051,1050,1052,1051,1052,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1052,1052,1051,1053,1052,1053,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1053,1053,1052,1054,1053,1054,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1054,1054,1053,1055,1054,1055,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1055,1055,1054,1056,1055,1056,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1056,1056,1055,1057,1056,1057,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1057,1057,1056,1058,1057,1058,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1058,1058,1057,1059,1058,1059,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1059,1059,1058,1060,1059,1060,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1060,1060,1059,1061,1060,1061,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1061,1061,1060,1062,1061,1062,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1062,1062,1061,1063,1062,1063,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1063,1063,1062,1064,1063,1064,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1064,1064,1063,1065,1064,1065,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1065,1065,1064,1066,1065,1066,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1066,1066,1065,1067,1066,1067,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1067,1067,1066,1068,1067,1068,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1068,1068,1067,1069,1068,1069,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1069,1069,1068,1070,1069,1070,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1070,1070,1069,1071,1070,1071,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1071,1071,1070,1072,1071,1072,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1072,1072,1071,1073,1072,1073,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1073,1073,1072,1074,1073,1074,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1074,1074,1073,1075,1074,1075,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1075,1075,1074,1076,1075,1076,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1076,1076,1075,1077,1076,1077,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1077,1077,1076,1078,1077,1078,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1078,1078,1077,1079,1078,1079,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1079,1079,1078,1080,1079,1080,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1080,1080,1079,1081,1080,1081,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1081,1081,1080,1082,1081,1082,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1082,1082,1081,1083,1082,1083,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1083,1083,1082,1084,1083,1084,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1084,1084,1083,1085,1084,1085,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1085,1085,1084,1086,1085,1086,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1086,1086,1085,1087,1086,1087,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1087,1087,1086,1088,1087,1088,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1088,1088,1087,1089,1088,1089,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1089,1089,1088,1090,1089,1090,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1090,1090,1089,1091,1090,1091,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1091,1091,1090,1092,1091,1092,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1092,1092,1091,1093,1092,1093,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1093,1093,1092,1094,1093,1094,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1094,1094,1093,1095,1094,1095,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1095,1095,1094,1096,1095,1096,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1096,1096,1095,1097,1096,1097,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1097,1097,1096,1098,1097,1098,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1098,1098,1097,1099,1098,1099,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1099,1099,1098,1100,1099,1100,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1100,1100,1099,1101,1100,1101,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1101,1101,1100,1102,1101,1102,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1102,1102,1101,1103,1102,1103,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1103,1103,1102,1104,1103,1104,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1104,1104,1103,1105,1104,1105,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1105,1105,1104,1106,1105,1106,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1106,1106,1105,1107,1106,1107,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1107,1107,1106,1108,1107,1108,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1108,1108,1107,1109,1108,1109,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1109,1109,1108,1110,1109,1110,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1110,1110,1109,1111,1110,1111,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1111,1111,1110,1112,1111,1112,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1112,1112,1111,1113,1112,1113,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1113,1113,1112,1114,1113,1114,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1114,1114,1113,1115,1114,1115,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1115,1115,1114,1116,1115,1116,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1116,1116,1115,1117,1116,1117,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1117,1117,1116,1118,1117,1118,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1118,1118,1117,1119,1118,1119,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1119,1119,1118,1120,1119,1120,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1120,1120,1119,1121,1120,1121,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1121,1121,1120,1122,1121,1122,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1122,1122,1121,1123,1122,1123,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1123,1123,1122,1124,1123,1124,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1124,1124,1123,1125,1124,1125,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1125,1125,1124,1126,1125,1126,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1126,1126,1125,1127,1126,1127,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1127,1127,1126,1128,1127,1128,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1128,1128,1127,1129,1128,1129,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1129,1129,1128,1130,1129,1130,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1130,1130,1129,1131,1130,1131,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1131,1131,1130,1132,1131,1132,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1132,1132,1131,1133,1132,1133,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1133,1133,1132,1134,1133,1134,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1134,1134,1133,1135,1134,1135,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1135,1135,1134,1136,1135,1136,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1136,1136,1135,1137,1136,1137,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1137,1137,1136,1138,1137,1138,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1138,1138,1137,1139,1138,1139,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1139,1139,1138,1140,1139,1140,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1140,1140,1139,1141,1140,1141,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1141,1141,1140,1142,1141,1142,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1142,1142,1141,1143,1142,1143,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1143,1143,1142,1144,1143,1144,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1144,1144,1143,1145,1144,1145,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1145,1145,1144,1146,1145,1146,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1146,1146,1145,1147,1146,1147,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1147,1147,1146,1148,1147,1148,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1148,1148,1147,1149,1148,1149,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1149,1149,1148,1150,1149,1150,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1150,1150,1149,1151,1150,1151,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1151,1151,1150,1152,1151,1152,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1152,1152,1151,1153,1152,1153,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1153,1153,1152,1154,1153,1154,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1154,1154,1153,1155,1154,1155,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1155,1155,1154,1156,1155,1156,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1156,1156,1155,1157,1156,1157,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1157,1157,1156,1158,1157,1158,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 ++1158,1158,1157,0,1158,1159,0,0,0,0,0,0,0,0,1.0,1,1,0,20,20,0,0,0 +diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv +new file mode 100644 +index 00000000..de7375dd +--- /dev/null ++++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/node.csv +@@ -0,0 +1,160 @@ ++NID,PID ++1001,1001 ++1002,1002 ++1003,1003 ++1004,1004 ++1005,1005 ++1006,1006 ++1007,1007 ++1008,1008 ++1009,1009 ++1010,1010 ++1011,1011 ++1012,1012 ++1013,1013 ++1014,1014 ++1015,1015 ++1016,1016 ++1017,1017 ++1018,1018 ++1019,1019 ++1020,1020 ++1021,1021 ++1022,1022 ++1023,1023 ++1024,1024 ++1025,1025 ++1026,1026 ++1027,1027 ++1028,1028 ++1029,1029 ++1030,1030 ++1031,1031 ++1032,1032 ++1033,1033 ++1034,1034 ++1035,1035 ++1036,1036 ++1037,1037 ++1038,1038 ++1039,1039 ++1040,1040 ++1041,1041 ++1042,1042 ++1043,1043 ++1044,1044 ++1045,1045 ++1046,1046 ++1047,1047 ++1048,1048 ++1049,1049 ++1050,1050 ++1051,1051 ++1052,1052 ++1053,1053 ++1054,1054 ++1055,1055 ++1056,1056 ++1057,1057 ++1058,1058 ++1059,1059 ++1060,1060 ++1061,1061 ++1062,1062 ++1063,1063 ++1064,1064 ++1065,1065 ++1066,1066 ++1067,1067 ++1068,1068 ++1069,1069 ++1070,1070 ++1071,1071 ++1072,1072 ++1073,1073 ++1074,1074 ++1075,1075 ++1076,1076 ++1077,1077 ++1078,1078 ++1079,1079 ++1080,1080 ++1081,1081 ++1082,1082 ++1083,1083 ++1084,1084 ++1085,1085 ++1086,1086 ++1087,1087 ++1088,1088 ++1089,1089 ++1090,1090 ++1091,1091 ++1092,1092 ++1093,1093 ++1094,1094 ++1095,1095 ++1096,1096 ++1097,1097 ++1098,1098 ++1099,1099 ++1100,1100 ++1101,1101 ++1102,1102 ++1103,1103 ++1104,1104 ++1105,1105 ++1106,1106 ++1107,1107 ++1108,1108 ++1109,1109 ++1110,1110 ++1111,1111 ++1112,1112 ++1113,1113 ++1114,1114 ++1115,1115 ++1116,1116 ++1117,1117 ++1118,1118 ++1119,1119 ++1120,1120 ++1121,1121 ++1122,1122 ++1123,1123 ++1124,1124 ++1125,1125 ++1126,1126 ++1127,1127 ++1128,1128 ++1129,1129 ++1130,1130 ++1131,1131 ++1132,1132 ++1133,1133 ++1134,1134 ++1135,1135 ++1136,1136 ++1137,1137 ++1138,1138 ++1139,1139 ++1140,1140 ++1141,1141 ++1142,1142 ++1143,1143 ++1144,1144 ++1145,1145 ++1146,1146 ++1147,1147 ++1148,1148 ++1149,1149 ++1150,1150 ++1151,1151 ++1152,1152 ++1153,1153 ++1154,1154 ++1155,1155 ++1156,1156 ++1157,1157 ++1158,1158 ++1159,1159 +diff --git a/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv +new file mode 100644 +index 00000000..0f778cae +--- /dev/null ++++ b/autoware.ai/autoware_files/vector_map/230209_carla_town04_straight/point.csv +@@ -0,0 +1,160 @@ ++PID,B(Lat),L(Long),H,Bx,Ly,ReF,MCODE1,MCODE2,MCODE3 ++1001,0,0,2.458,127.057,258.58,0,0,0,0 ++1002,0,0,2.459,128.123,258.573,0,0,0,0 ++1003,0,0,2.459,129.339,258.569,0,0,0,0 ++1004,0,0,2.459,130.567,258.567,0,0,0,0 ++1005,0,0,2.46,131.7,258.559,0,0,0,0 ++1006,0,0,2.462,132.796,258.564,0,0,0,0 ++1007,0,0,2.462,134.128,258.56,0,0,0,0 ++1008,0,0,2.461,135.17,258.551,0,0,0,0 ++1009,0,0,2.463,136.475,258.542,0,0,0,0 ++1010,0,0,2.463,137.611,258.537,0,0,0,0 ++1011,0,0,2.463,138.785,258.533,0,0,0,0 ++1012,0,0,2.464,139.891,258.535,0,0,0,0 ++1013,0,0,2.462,141.114,258.527,0,0,0,0 ++1014,0,0,2.461,142.477,258.524,0,0,0,0 ++1015,0,0,2.461,143.766,258.511,0,0,0,0 ++1016,0,0,2.461,145.134,258.511,0,0,0,0 ++1017,0,0,2.461,146.549,258.506,0,0,0,0 ++1018,0,0,2.461,147.877,258.499,0,0,0,0 ++1019,0,0,2.46,149.143,258.5,0,0,0,0 ++1020,0,0,2.459,150.371,258.491,0,0,0,0 ++1021,0,0,2.457,151.52,258.49,0,0,0,0 ++1022,0,0,2.454,152.64,258.49,0,0,0,0 ++1023,0,0,2.457,153.716,258.492,0,0,0,0 ++1024,0,0,2.46,154.738,258.489,0,0,0,0 ++1025,0,0,2.457,156.035,258.492,0,0,0,0 ++1026,0,0,2.455,157.264,258.486,0,0,0,0 ++1027,0,0,2.455,158.48,258.478,0,0,0,0 ++1028,0,0,2.454,159.704,258.475,0,0,0,0 ++1029,0,0,2.455,160.865,258.469,0,0,0,0 ++1030,0,0,2.456,161.939,258.464,0,0,0,0 ++1031,0,0,2.48,163.143,258.449,0,0,0,0 ++1032,0,0,2.51,164.181,258.443,0,0,0,0 ++1033,0,0,2.518,165.358,258.431,0,0,0,0 ++1034,0,0,2.548,166.424,258.413,0,0,0,0 ++1035,0,0,2.557,167.449,258.416,0,0,0,0 ++1036,0,0,2.556,168.57,258.406,0,0,0,0 ++1037,0,0,2.557,169.594,258.401,0,0,0,0 ++1038,0,0,2.556,170.677,258.394,0,0,0,0 ++1039,0,0,2.56,171.822,258.386,0,0,0,0 ++1040,0,0,2.557,173.021,258.379,0,0,0,0 ++1041,0,0,2.559,174.297,258.381,0,0,0,0 ++1042,0,0,2.543,175.655,258.373,0,0,0,0 ++1043,0,0,2.508,176.851,258.383,0,0,0,0 ++1044,0,0,2.501,177.954,258.379,0,0,0,0 ++1045,0,0,2.47,179.003,258.372,0,0,0,0 ++1046,0,0,2.452,180.062,258.379,0,0,0,0 ++1047,0,0,2.454,181.096,258.374,0,0,0,0 ++1048,0,0,2.455,182.411,258.374,0,0,0,0 ++1049,0,0,2.455,183.703,258.375,0,0,0,0 ++1050,0,0,2.454,184.753,258.374,0,0,0,0 ++1051,0,0,2.451,185.757,258.374,0,0,0,0 ++1052,0,0,2.455,187.03,258.374,0,0,0,0 ++1053,0,0,2.454,188.244,258.368,0,0,0,0 ++1054,0,0,2.456,189.383,258.367,0,0,0,0 ++1055,0,0,2.457,190.667,258.367,0,0,0,0 ++1056,0,0,2.458,191.863,258.363,0,0,0,0 ++1057,0,0,2.459,193.009,258.364,0,0,0,0 ++1058,0,0,2.46,194.084,258.356,0,0,0,0 ++1059,0,0,2.458,195.18,258.357,0,0,0,0 ++1060,0,0,2.458,196.497,258.35,0,0,0,0 ++1061,0,0,2.459,197.731,258.351,0,0,0,0 ++1062,0,0,2.459,198.941,258.347,0,0,0,0 ++1063,0,0,2.462,200.026,258.341,0,0,0,0 ++1064,0,0,2.458,201.086,258.338,0,0,0,0 ++1065,0,0,2.459,202.086,258.335,0,0,0,0 ++1066,0,0,2.458,203.396,258.327,0,0,0,0 ++1067,0,0,2.455,204.465,258.329,0,0,0,0 ++1068,0,0,2.456,205.481,258.32,0,0,0,0 ++1069,0,0,2.458,206.775,258.318,0,0,0,0 ++1070,0,0,2.458,207.835,258.307,0,0,0,0 ++1071,0,0,2.457,209.0,258.303,0,0,0,0 ++1072,0,0,2.457,210.085,258.302,0,0,0,0 ++1073,0,0,2.46,211.148,258.302,0,0,0,0 ++1074,0,0,2.457,212.268,258.293,0,0,0,0 ++1075,0,0,2.456,213.408,258.297,0,0,0,0 ++1076,0,0,2.457,214.507,258.287,0,0,0,0 ++1077,0,0,2.455,215.509,258.286,0,0,0,0 ++1078,0,0,2.458,216.589,258.283,0,0,0,0 ++1079,0,0,2.458,217.782,258.281,0,0,0,0 ++1080,0,0,2.457,218.88,258.272,0,0,0,0 ++1081,0,0,2.456,219.968,258.274,0,0,0,0 ++1082,0,0,2.457,221.04,258.268,0,0,0,0 ++1083,0,0,2.457,222.66,258.256,0,0,0,0 ++1084,0,0,2.457,223.777,258.255,0,0,0,0 ++1085,0,0,2.456,224.837,258.245,0,0,0,0 ++1086,0,0,2.458,225.871,258.242,0,0,0,0 ++1087,0,0,2.456,227.518,258.236,0,0,0,0 ++1088,0,0,2.455,228.57,258.227,0,0,0,0 ++1089,0,0,2.457,229.861,258.23,0,0,0,0 ++1090,0,0,2.455,231.087,258.222,0,0,0,0 ++1091,0,0,2.455,232.231,258.214,0,0,0,0 ++1092,0,0,2.455,233.329,258.223,0,0,0,0 ++1093,0,0,2.454,234.347,258.208,0,0,0,0 ++1094,0,0,2.453,235.55,258.218,0,0,0,0 ++1095,0,0,2.453,236.672,258.2,0,0,0,0 ++1096,0,0,2.454,237.704,258.226,0,0,0,0 ++1097,0,0,2.452,238.879,258.206,0,0,0,0 ++1098,0,0,2.451,239.912,258.206,0,0,0,0 ++1099,0,0,2.448,241.081,258.194,0,0,0,0 ++1100,0,0,2.446,242.165,258.194,0,0,0,0 ++1101,0,0,2.438,243.309,258.195,0,0,0,0 ++1102,0,0,2.443,244.471,258.187,0,0,0,0 ++1103,0,0,2.443,245.583,258.172,0,0,0,0 ++1104,0,0,2.442,246.585,258.165,0,0,0,0 ++1105,0,0,2.442,247.906,258.162,0,0,0,0 ++1106,0,0,2.442,248.917,258.165,0,0,0,0 ++1107,0,0,2.442,250.01,258.169,0,0,0,0 ++1108,0,0,2.44,251.104,258.163,0,0,0,0 ++1109,0,0,2.44,252.201,258.177,0,0,0,0 ++1110,0,0,2.442,253.26,258.17,0,0,0,0 ++1111,0,0,2.443,254.412,258.154,0,0,0,0 ++1112,0,0,2.446,255.422,258.15,0,0,0,0 ++1113,0,0,2.448,256.615,258.149,0,0,0,0 ++1114,0,0,2.45,257.748,258.141,0,0,0,0 ++1115,0,0,2.45,259.226,258.121,0,0,0,0 ++1116,0,0,2.451,260.477,258.14,0,0,0,0 ++1117,0,0,2.449,261.941,258.131,0,0,0,0 ++1118,0,0,2.451,263.243,258.124,0,0,0,0 ++1119,0,0,2.45,264.476,258.12,0,0,0,0 ++1120,0,0,2.449,265.628,258.112,0,0,0,0 ++1121,0,0,2.453,266.706,258.109,0,0,0,0 ++1122,0,0,2.451,267.735,258.11,0,0,0,0 ++1123,0,0,2.449,268.913,258.104,0,0,0,0 ++1124,0,0,2.45,270.088,258.118,0,0,0,0 ++1125,0,0,2.45,271.19,258.112,0,0,0,0 ++1126,0,0,2.451,272.23,258.118,0,0,0,0 ++1127,0,0,2.451,273.427,258.112,0,0,0,0 ++1128,0,0,2.452,274.577,258.102,0,0,0,0 ++1129,0,0,2.449,275.678,258.107,0,0,0,0 ++1130,0,0,2.45,276.814,258.097,0,0,0,0 ++1131,0,0,2.449,277.887,258.094,0,0,0,0 ++1132,0,0,2.445,279.118,258.09,0,0,0,0 ++1133,0,0,2.448,280.141,258.089,0,0,0,0 ++1134,0,0,2.449,281.253,258.081,0,0,0,0 ++1135,0,0,2.449,282.266,258.075,0,0,0,0 ++1136,0,0,2.447,283.271,258.072,0,0,0,0 ++1137,0,0,2.448,284.813,258.059,0,0,0,0 ++1138,0,0,2.45,286.002,258.055,0,0,0,0 ++1139,0,0,2.451,287.101,258.044,0,0,0,0 ++1140,0,0,2.45,288.262,258.043,0,0,0,0 ++1141,0,0,2.452,289.275,258.045,0,0,0,0 ++1142,0,0,2.451,290.578,258.039,0,0,0,0 ++1143,0,0,2.452,291.778,258.032,0,0,0,0 ++1144,0,0,2.451,292.913,258.017,0,0,0,0 ++1145,0,0,2.451,294.195,258.018,0,0,0,0 ++1146,0,0,2.453,295.515,258.024,0,0,0,0 ++1147,0,0,2.449,296.713,258.02,0,0,0,0 ++1148,0,0,2.453,297.854,258.008,0,0,0,0 ++1149,0,0,2.454,298.938,258.016,0,0,0,0 ++1150,0,0,2.453,300.189,258.023,0,0,0,0 ++1151,0,0,2.452,301.196,258.02,0,0,0,0 ++1152,0,0,2.451,302.721,258.009,0,0,0,0 ++1153,0,0,2.45,304.159,257.989,0,0,0,0 ++1154,0,0,2.444,305.249,257.989,0,0,0,0 ++1155,0,0,2.444,306.501,257.979,0,0,0,0 ++1156,0,0,2.446,307.685,257.976,0,0,0,0 ++1157,0,0,2.444,308.786,257.972,0,0,0,0 ++1158,0,0,2.44,309.827,257.975,0,0,0,0 ++1159,0,0,2.442,311.051,257.963,0,0,0,0 +diff --git a/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h b/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h +index 323aeea3..2cd7f757 100755 +--- a/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h ++++ b/autoware.ai/src/autoware/common/op_planner/include/op_planner/RoadNetwork.h +@@ -908,6 +908,14 @@ public: + double smoothingSmoothWeight; + double smoothingToleranceError; + ++ // Added by HYP ++ int enableDebug; ++ ++ // Added by HYP for blocking the trajectories ++ double lateralBlockingThreshold; ++ double frontLongitudinalBlockingThreshold; ++ double rearLongitudinalBlockingThreshold; ++ + // Added by HJW for make traj eval parameter + double weightPriority; + double weightTransition; +@@ -975,11 +983,17 @@ public: + smoothingSmoothWeight = 0.2; + smoothingToleranceError = 0.05; + +- double weightPriority = 1; +- double weightTransition = 1; +- double weightLong = 1.2; +- double weightLat = 1; +- double LateralSkipDistance = 5; ++ enableDebug = 0; ++ ++ lateralBlockingThreshold = 1.5; ++ frontLongitudinalBlockingThreshold = 30.0; ++ rearLongitudinalBlockingThreshold = -5.0; ++ ++ weightPriority = 1; ++ weightTransition = 1; ++ weightLong = 1.2; ++ weightLat = 1; ++ LateralSkipDistance = 5; + + stopSignStopTime = 2.0; + +diff --git a/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h b/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h +index 05d60262..84d5b056 100755 +--- a/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h ++++ b/autoware.ai/src/autoware/common/op_planner/include/op_planner/TrajectoryDynamicCosts.h +@@ -58,7 +58,7 @@ public: + private: + bool ValidateRollOutsInput(const vector > >& rollOuts); + vector CalculatePriorityAndLaneChangeCosts(const vector >& laneRollOuts, const int& lane_index, const PlanningParams& params); +- void NormalizeCosts(vector& trajectoryCosts); ++ void NormalizeCosts(vector& trajectoryCosts, int enableDebug); + void CalculateLateralAndLongitudinalCosts(vector& trajectoryCosts, const vector > >& rollOuts, const vector >& totalPaths, const WayPoint& currState, const vector& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState); + void CalculateLateralAndLongitudinalCostsStatic(vector& trajectoryCosts, const vector >& rollOuts, const vector& totalPaths, const WayPoint& currState, const vector& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState); + void CalculateTransitionCosts(vector& trajectoryCosts, const int& currTrajectoryIndex, const PlanningParams& params); +diff --git a/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp b/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp +index 9edfca2f..d1857f97 100755 +--- a/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp ++++ b/autoware.ai/src/autoware/common/op_planner/src/DecisionMaker.cpp +@@ -667,7 +667,7 @@ bool DecisionMaker::SelectSafeTrajectory() + desiredVelocity = m_params.maxSpeed; + } + +- if(desiredVelocity < m_params.maxSpeed * m_params.curveVelocityRatio){ // minimum of target velocity is max_speed / 2 ++ if(desiredVelocity < m_params.maxSpeed * m_params.curveVelocityRatio){ // minimum of target velocity is max_speed * curveVelocityRatio + desiredVelocity = m_params.maxSpeed * m_params.curveVelocityRatio; + } + previous_velocity = desiredVelocity; +@@ -739,6 +739,7 @@ bool DecisionMaker::SelectSafeTrajectory() + const TrajectoryCost& tc, + const bool& bEmergencyStop) + { ++ static double prev_max_velocity = 0.0; + + PlannerHNS::BehaviorState beh; + state = currPose; +@@ -766,7 +767,7 @@ bool DecisionMaker::SelectSafeTrajectory() + beh.bNewPlan = SelectSafeTrajectory(); + + beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt); +- ++ + //std::cout << "Eval_i: " << tc.index << ", Curr_i: " << m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << ", Prev_i: " << m_pCurrentBehaviorState->GetCalcParams()->iPrevSafeTrajectory << std::endl; + + return beh; +diff --git a/autoware.ai/src/autoware/common/op_planner/src/TrajectoryDynamicCosts.cpp b/autoware.ai/src/autoware/common/op_planner/src/TrajectoryDynamicCosts.cpp +index 6e563dad..0c508543 100755 +--- a/autoware.ai/src/autoware/common/op_planner/src/TrajectoryDynamicCosts.cpp ++++ b/autoware.ai/src/autoware/common/op_planner/src/TrajectoryDynamicCosts.cpp +@@ -207,7 +207,7 @@ TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector 0 && rollOuts.at(0).size()>0) +@@ -434,9 +431,7 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector params.minFollowingDistance || lateralDist > m_LateralSkipDistance) +@@ -494,7 +487,9 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector= -5 && longitudinalDist < 30){ ++ if( lateralDist <= params.lateralBlockingThreshold ++ && longitudinalDist < params.frontLongitudinalBlockingThreshold ++ && longitudinalDist >= params.rearLongitudinalBlockingThreshold){ + trajectoryCosts.at(it).bBlocked = true; + } + +@@ -517,17 +512,15 @@ void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector& trajectoryCosts) ++void TrajectoryDynamicCosts::NormalizeCosts(vector& trajectoryCosts, int enableDebug) + { + //Normalize costs + double totalPriorities = 0; +@@ -712,22 +705,19 @@ void TrajectoryDynamicCosts::NormalizeCosts(vector& trajectoryCo + + trajectoryCosts.at(ic).cost = (m_WeightPriority*trajectoryCosts.at(ic).priority_cost + m_WeightTransition*trajectoryCosts.at(ic).transition_cost + m_WeightLat*trajectoryCosts.at(ic).lateral_cost + m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost)/4.0; + +- #ifdef DEBUG_ENABLE +- std::cout << "Index: " << ic +- << ", Priority: " << trajectoryCosts.at(ic).priority_cost +- << ", Transition: " << trajectoryCosts.at(ic).transition_cost +- << ", Lat: " << trajectoryCosts.at(ic).lateral_cost +- << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost +- << ", Change: " << trajectoryCosts.at(ic).lane_change_cost +- << ", Avg: " << trajectoryCosts.at(ic).cost +- << ", Blocked : " << trajectoryCosts.at(ic).bBlocked +- << std::endl; +- #endif ++ if(enableDebug){ ++ std::cout << "Index: " << ic ++ << ", Priority: " << trajectoryCosts.at(ic).priority_cost ++ << ", Transition: " << trajectoryCosts.at(ic).transition_cost ++ << ", Lat: " << trajectoryCosts.at(ic).lateral_cost ++ << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost ++ << ", Change: " << trajectoryCosts.at(ic).lane_change_cost ++ << ", Avg: " << trajectoryCosts.at(ic).cost ++ << ", Blocked : " << trajectoryCosts.at(ic).bBlocked ++ << std::endl; ++ } + } +- +- #ifdef DEBUG_ENABLE +- std::cout << "------------------------ " << std::endl; +- #endif ++ if(enableDebug) std::cout << "------------------------ " << std::endl; + } + + vector TrajectoryDynamicCosts::CalculatePriorityAndLaneChangeCosts(const vector >& laneRollOuts, +diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common.hpp b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common.hpp +deleted file mode 100644 +index dd7b197b..00000000 +--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common.hpp ++++ /dev/null +@@ -1,8 +0,0 @@ +-// Task state +-#define TASK_STATE_READY 0 +-#define TASK_STATE_RUNNING 1 +-#define TASK_STATE_DONE 2 +-#define GPU_SEG_LOOP_START 0 +-#define GPU_SEG_LOOP_MID 1 +-#define GPU_SEG_LOOP_END 2 +-#define RUBIS_NO_INSTANCE 0 +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common_c.h b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common_c.h +deleted file mode 100644 +index 52711361..00000000 +--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/common_c.h ++++ /dev/null +@@ -1,10 +0,0 @@ +-// Task state +-#define TASK_STATE_READY 0 +-#define TASK_STATE_RUNNING 1 +-#define TASK_STATE_DONE 2 +- +-#define GPU_SEG_LOOP_START 0 +-#define GPU_SEG_LOOP_MID 1 +-#define GPU_SEG_LOOP_END 2 +- +-#define RUBIS_NO_INSTANCE 0 +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched.hpp b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched.hpp +index 496d3899..c9b069ec 100644 +--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched.hpp ++++ b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched.hpp +@@ -55,7 +55,6 @@ + #define TASK_READY 1 + + namespace rubis { +-namespace sched { + + // contains thread-specific arguments + struct thr_arg { +@@ -91,61 +90,31 @@ struct sched_attr { + __u64 sched_period; + }; + +-// GPU +-typedef struct gpuSchedInfo{ +- int pid; +- unsigned long deadline; +- int state; // NONE = 0, WAIT = 1, RUN = 2 +- int scheduling_flag; +-} GPUSchedInfo; +- + extern int key_id_; + extern int is_scheduled_; +-extern int gpu_scheduling_flag_; +-extern GPUSchedInfo* gpu_sched_info_; +-extern int gpu_scheduler_pid_; + extern std::string task_filename_; +-extern std::string gpu_deadline_filename_; +-// extern unsigned long gpu_deadline_list_[1024]; +-extern unsigned long* gpu_deadline_list_; +-extern unsigned int max_gpu_id_; +-extern unsigned int gpu_seg_id_; +-extern unsigned int cpu_seg_id_; +-extern int task_state_; +-extern int is_task_ready_; +-extern int was_in_loop_; +-extern int loop_cnt_; +-extern int gpu_seg_cnt_in_loop_; +- +-// Task scheduling ++ + int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags); + int sched_getattr(pid_t pid, struct sched_attr *attr, unsigned int size, unsigned int flags); +-bool set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _period); +-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline); ++ ++bool set_sched_deadline(int pid, unsigned int exec_time, unsigned int deadline, unsigned int period); ++bool set_sched_fifo(int pid, int priority); ++bool set_sched_fifo(int pid, int priority, int child_priority); ++bool set_sched_rr(int pid, int priority); ++bool set_sched_rr(int pid, int priority, int child_priority); ++ ++bool init_task_scheduling(std::string policy, struct sched_attr attr); + void yield_task_scheduling(); +-void init_task(); +-void disable_task(); ++struct sched_attr create_sched_attr(int priority, int exec_time, int deadline, int period); + +-// GPU scheduling +-void init_gpu_scheduling(std::string task_filename, std::string gpu_deadline_filename, int key_id); +-void get_deadline_list(); + void sig_handler(int signum); + void termination(); + unsigned long get_current_time_us(); + ++std::string get_cmd_output(const char* cmd); ++std::vector get_child_pids(int pid); ++std::vector tokenize_string(std::string s, std::string delimiter); + +-void start_job(); +-void finish_job(); +- +-void request_gpu(); +-void request_gpu_in_loop(int flag); +- +-void yield_gpu(std::string remark = ""); +-void yield_gpu_in_loop(int flag, std::string remark = ""); +-void print_loop_info(std::string tag); +-void print_gpu_deadline_list(); +- +-} // namespace sched + } // namespace rubis + + #endif +diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_c.h b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_c.h +index 986c680c..60af8e3f 100644 +--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_c.h ++++ b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_c.h +@@ -89,58 +89,18 @@ struct sched_attr { + __u64 sched_period; + }; + +-// GPU +-typedef struct gpuSchedInfo{ +- int pid; +- unsigned long deadline; +- int state; // NONE = 0, WAIT = 1, RUN = 2 +- int scheduling_flag; +-} GPUSchedInfo; +- + extern int key_id_; + extern int is_scheduled_; +-extern int gpu_scheduling_flag_; +-extern GPUSchedInfo* gpu_sched_info_; +-extern int gpu_scheduler_pid_; + extern char* task_filename_; +-extern char* gpu_deadline_filename_; +-// extern unsigned long gpu_deadline_list_[1024]; +-extern unsigned long* gpu_deadline_list_; +-extern unsigned int max_gpu_id_; +-extern unsigned int gpu_seg_id_; +-extern unsigned int cpu_seg_id_; +-extern int task_state_; +-extern int is_task_ready_; +-extern int was_in_loop_; +-extern int loop_cnt_; +-extern int gpu_seg_cnt_in_loop_; + + // Task scheduling + // int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags); + // int sched_getattr(pid_t pid, struct sched_attr *attr, unsigned int size, unsigned int flags); + int set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _period); +-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline); + void yield_task_scheduling(); +-void init_task(); +-void disable_task(); + +-// GPU scheduling +-void init_gpu_scheduling(char* task_filename, char* gpu_deadline_filename, int key_id); +-void get_deadline_list(); + void sig_handler(int signum); + void termination(); + unsigned long get_current_time_us(); + +-void start_job(); +-void finish_job(); +- +-void request_gpu(); +-void yield_gpu(); +-void yield_gpu_with_remark(const char* remark); +- +-void request_gpu_in_loop(int flag); +-void yield_gpu_in_loop(int flag); +-void yield_gpu_with_remark_in_loop(int flag, const char* remark); +-void print_loop_info(const char* tag); +-void print_gpu_deadline_list(); + #endif +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling.hpp b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling.hpp +index f428b32a..68ff4580 100644 +--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling.hpp ++++ b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling.hpp +@@ -9,44 +9,20 @@ + #include + #include + #include +-#include + + namespace rubis { +- extern int instance_mode_; + extern unsigned long instance_; +- +-namespace sched { +- extern int task_profiling_flag_; +- extern int gpu_profiling_flag_; ++ extern unsigned long obj_instance_; + + extern FILE* task_response_time_fp_; +- extern FILE* seg_execution_time_fp_; +- extern FILE* seg_response_time_fp_; +- +- extern int is_gpu_profiling_started_; + + extern struct timespec task_start_time_; + extern struct timespec task_end_time_; +- extern unsigned long gpu_seg_response_time_; +- extern unsigned long gpu_seg_execution_time_; +- extern unsigned long cpu_seg_response_time_; +- extern int is_gpu_profiling_ready_; + + void init_task_profiling(std::string task_reponse_time_filename); + void start_task_profiling(); + void stop_task_profiling(unsigned long instance, int state); +- void init_gpu_profiling(std::string execution_time_filename, std::string response_time_filename); +- void start_profiling_cpu_seg_response_time(); +- void stop_profiling_cpu_seg_response_time(unsigned int cpu_seg_id, unsigned int iter); +- void start_profiling_gpu_seg_response_time(); +- void start_profiling_gpu_seg_execution_time(); +- void stop_profiling_gpu_seg_time(unsigned int gpu_seg_id, unsigned int iter, std::string remark = " "); + unsigned long get_current_time_ns(); +- void start_job_profiling(); +- void finish_job_profiling(unsigned int cpu_seg_id); +- void start_gpu_profiling(); +- +-} + } + + #endif +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling_c.h b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling_c.h +index 4925accf..2f101036 100644 +--- a/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling_c.h ++++ b/autoware.ai/src/autoware/common/rubis_lib/include/rubis_lib/sched_profiling_c.h +@@ -10,28 +10,16 @@ + #include + #include + #include +-#include + + #define rubis_lib_BUFFER_SIZE 1024 + +-extern int instance_mode_; + extern unsigned long instance_; +- +-extern int task_profiling_flag_; +-extern int gpu_profiling_flag_; ++extern unsigned long obj_instance_; + + extern FILE* task_response_time_fp_; +-extern FILE* seg_execution_time_fp_; +-extern FILE* seg_response_time_fp_; +- +-extern int is_gpu_profiling_started_; + + extern struct timespec task_start_time_; + extern struct timespec task_end_time_; +-extern unsigned long gpu_seg_response_time_; +-extern unsigned long gpu_seg_execution_time_; +-extern unsigned long cpu_seg_response_time_; +-extern int is_gpu_profiling_ready_; + + #ifdef __cplusplus + extern "C"{ +@@ -40,17 +28,7 @@ extern "C"{ + void init_task_profiling(char* _task_response_time_filename); + void start_task_profiling(); + void stop_task_profiling(unsigned long instance, int state); +-void init_gpu_profiling(char* _execution_time_filename, char* _response_time_filename); +-void start_profiling_cpu_seg_response_time(); +-void stop_profiling_cpu_seg_response_time(unsigned int cpu_seg_id, unsigned int iter); +-void start_profiling_gpu_seg_response_time(); +-void start_profiling_gpu_seg_execution_time(); +-void stop_profiling_gpu_seg_time(unsigned int gpu_seg_id, unsigned int iter); +-void stop_profiling_gpu_seg_time_with_remark(unsigned int gpu_seg_id, unsigned int iter, const char* remark); + unsigned long get_current_time_ns(); +-void start_job_profiling(); +-void finish_job_profiling(unsigned int cpu_seg_id); +-void start_gpu_profiling(); + + #ifdef __cplusplus + } +diff --git a/autoware.ai/src/autoware/common/rubis_lib/src/sched.cpp b/autoware.ai/src/autoware/common/rubis_lib/src/sched.cpp +index 4d41e5c3..3eaf336c 100644 +--- a/autoware.ai/src/autoware/common/rubis_lib/src/sched.cpp ++++ b/autoware.ai/src/autoware/common/rubis_lib/src/sched.cpp +@@ -3,26 +3,21 @@ + // #define DEBUG + + namespace rubis{ +-namespace sched{ + + int key_id_; + int is_scheduled_; +-int gpu_scheduling_flag_; +-GPUSchedInfo* gpu_sched_info_; +-int gpu_scheduler_pid_; + std::string task_filename_; +-std::string gpu_deadline_filename_; +-// unsigned long gpu_deadline_list_[1024]; +-unsigned long* gpu_deadline_list_; +-unsigned int max_gpu_id_ = 0; +-unsigned int cpu_seg_id_ = 0; +-unsigned int gpu_seg_id_ = 0; +- +-int is_task_ready_ = TASK_NOT_READY; +-int task_state_ = TASK_STATE_READY; +-int was_in_loop_ = 0; +-int loop_cnt_ = 0; +-int gpu_seg_cnt_in_loop_ = 0; ++ ++struct sched_attr create_sched_attr(int priority, int exec_time, int deadline, int period){ ++ struct sched_attr attr; ++ ++ attr.sched_priority = (__u32)priority; ++ attr.sched_runtime = (__u64)exec_time; ++ attr.sched_deadline = (__u64)deadline; ++ attr.sched_period = (__u64)period; ++ ++ return attr; ++} + + // system call hook to call SCHED_DEADLINE + int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags){ +@@ -34,354 +29,168 @@ int sched_getattr(pid_t pid, struct sched_attr *attr, unsigned int size, unsigne + return syscall(__NR_sched_getattr, pid, attr, size, flags); + } + +-bool set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _period) { +- struct sched_attr attr; +- attr.size = sizeof(attr); +- attr.sched_flags = 0; +- attr.sched_nice = 0; +- attr.sched_priority = 0; +- +- attr.sched_policy = SCHED_DEADLINE; // 6 +- attr.sched_runtime = _exec_time; +- attr.sched_deadline = _deadline; +- attr.sched_period = _period; +- +- int ret = sched_setattr(_tid, &attr, attr.sched_flags); +- if(ret < 0) { +- std::cerr << "[ERROR] sched_setattr failed. Are you root? (" << ret << ")" << std::endl; +- perror("sched_setattr"); +- exit(-1); +- return false; +- } +- // else { +- // std::cerr << "[SCHED_DEADLINE] (" << _tid << ") exec_time: " << _exec_time << " _deadline: " << _deadline << " _period: " << _period << std::endl; +- // } +- return true; +-} +- +-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline){ +- sched::set_sched_deadline(gettid(), +- static_cast(task_execution_time), +- static_cast(task_relative_deadline), +- static_cast(task_minimum_inter_release_time) +- ); +-} +- +-void yield_task_scheduling(){ +- sched_yield(); +-} +- +-void init_gpu_scheduling(std::string task_filename, std::string gpu_deadline_filename, int key_id){ +- gpu_scheduling_flag_ = 1; +- task_filename_ = task_filename; +- gpu_deadline_filename_ = gpu_deadline_filename; +- key_id_ = key_id; +- +- // Get deadlines +- printf("deadline filename: %s\n", gpu_deadline_filename_.c_str()); +- get_deadline_list(); +- +- // Init signal handler +- signal(SIGINT, sig_handler); +- signal(SIGTSTP, sig_handler); +- signal(SIGQUIT, sig_handler); +- +- // Create task file +- FILE* task_fp; +- task_fp = fopen(task_filename_.c_str(), "w"); +- if(task_fp == NULL){ +- printf("Cannot create task file at %s\n", task_filename_.c_str()); +- exit(1); +- } +- fprintf(task_fp, "%d\n", getpid()); +- fprintf(task_fp, "%d", key_id_); +- fclose(task_fp); +- +- // Get pid of scheduler +- FILE* scheduler_fp; +- printf("Wait the scheduler...\n"); +- +- while(1){ +- scheduler_fp = fopen("/tmp/np_edf_scheduler", "r"); +- if(scheduler_fp) break; +- } +- while(1){ +- fscanf(scheduler_fp, "%d", &gpu_scheduler_pid_); +- if(gpu_scheduler_pid_ != 0) break; ++bool set_sched_fifo(int pid, int priority){ ++ struct sched_param sp = {.sched_priority = (int32_t) priority}; ++ int ret = sched_setscheduler(pid, SCHED_FIFO, &sp); ++ if(ret == -1){ ++ perror("sched_setscheduler"); ++ return false; + } +- printf("Scheduler pid: %d\n", gpu_scheduler_pid_); +- fclose(scheduler_fp); ++ return true; ++} + ++bool set_sched_fifo(int pid, int priority, int child_priority){ ++ if(pid == 0) pid = getpid(); ++ bool output = set_sched_fifo(pid, priority); ++ std::vector child_pids = get_child_pids(pid); + +- // Initialize scheduling information (shared memory data) +- FILE* sm_key_fp; +- sm_key_fp = fopen("/tmp/sm_key", "r"); +- if(sm_key_fp == NULL){ +- printf("Cannot open /tmp/sm_key\n"); +- termination(); ++ for(auto it = child_pids.begin(); it != child_pids.end(); it++){ ++ int child_pid = *it; ++ output = set_sched_fifo(child_pid, child_priority); + } + +- key_t key; +- int shmid; +- key = ftok("/tmp/sm_key", key_id_); +- shmid = shmget(key, sizeof(GPUSchedInfo), 0666|IPC_CREAT); +- gpu_sched_info_ = (GPUSchedInfo*)shmat(shmid, 0, 0); +- gpu_sched_info_->pid = getpid(); +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- gpu_sched_info_->scheduling_flag = 0; +- printf("Task [%d] is ready to work\n", getpid()); +- +- gpu_seg_id_ = 0; +- cpu_seg_id_ = 0; +- +- return; ++ return output; + } + +-void sig_handler(int signum){ +- if(signum == SIGINT || signum == SIGTSTP || signum == SIGQUIT){ +- termination(); ++bool set_sched_rr(int pid, int priority){ ++ struct sched_param sp = {.sched_priority = (int32_t) priority}; ++ int ret = sched_setscheduler(pid, SCHED_RR, &sp); ++ if(ret == -1){ ++ perror("sched_setscheduler"); ++ return false; + } ++ return true; + } + +-void get_deadline_list(){ +- if(gpu_deadline_filename_.at(0) == '~'){ +- gpu_deadline_filename_.erase(0, 1); +- std::string user_home_str(std::getenv("USER_HOME")); +- gpu_deadline_filename_ = user_home_str + gpu_deadline_filename_; +- } ++bool set_sched_rr(int pid, int priority, int child_priority){ ++ if(pid == 0) pid = getpid(); ++ bool output = set_sched_rr(pid, priority); ++ std::vector child_pids = get_child_pids(pid); + +- FILE* fp; +- fp = fopen(gpu_deadline_filename_.c_str(), "r"); +- if(fp==NULL){ +- fprintf(stderr, "Cannot find file %s\n", gpu_deadline_filename_.c_str()); +- exit(1); ++ for(auto it = child_pids.begin(); it != child_pids.end(); it++){ ++ int child_pid = *it; ++ output = set_sched_rr(child_pid, child_priority); + } + +- char* buf; +- int cnt = 0; +- size_t len = 0; +- ssize_t n; ++ return output; ++} + +- getline(&buf, &len, fp); // skip first line +- while( (n = getline(&buf, &len, fp)) != -1 ){ +- cnt++; +- } +- max_gpu_id_ = cnt; +- gpu_deadline_list_ = (unsigned long *)malloc(sizeof(unsigned long) * max_gpu_id_); +- rewind(fp); ++bool set_sched_deadline(int pid, unsigned int exec_time, unsigned int deadline, unsigned int period) { + +- int idx = 0; +- +- getline(&buf, &len, fp); // skip first line +- while((n = getline(&buf, &len, fp)) != -1){ +- unsigned long deadline; +- sscanf(buf, "gpu_%*d,%llu", &deadline); +- gpu_deadline_list_[idx++] = deadline; +- } ++ struct sched_attr attr; ++ attr.size = sizeof(attr); ++ attr.sched_flags = 0; ++ attr.sched_nice = 0; ++ attr.sched_priority = 0; + +- // print_gpu_deadline_list(); ++ attr.sched_policy = SCHED_DEADLINE; // 6 ++ attr.sched_runtime = (__u64)exec_time; ++ attr.sched_deadline = (__u64)deadline; ++ attr.sched_period = (__u64)period; + +- free(buf); // free allocated memory at getline +- fclose(fp); +- +- return; ++ int ret = sched_setattr(pid, &attr, attr.sched_flags); ++ if(ret < 0) { ++ std::cerr << "[ERROR] sched_setattr failed. Are you root? (" << ret << ")" << std::endl; ++ perror("sched_setattr"); ++ exit(-1); ++ return false; ++ } ++ // else { ++ // std::cerr << "[SCHED_DEADLINE] (" << _pid << ") exec_time: " << _exec_time << " _deadline: " << _deadline << " _period: " << _period << std::endl; ++ // } ++ return true; + } + +-void termination(){ +- printf("TERMINATION\n"); +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->state = SCHEDULING_STATE_STOP; +- shmdt(gpu_sched_info_); +- } +- +- free(gpu_deadline_list_); +- if(remove(task_filename_.c_str())){ +- printf("Cannot remove file %s\n", task_filename_); +- exit(1); +- } +- +- if(task_profiling_flag_){ +- fclose(task_response_time_fp_); ++bool init_task_scheduling(std::string policy, struct sched_attr attr){ ++ if(policy.compare(std::string("NONE")) == 0){ ++ return true; + } +- +- if(gpu_profiling_flag_){ +- fclose(seg_response_time_fp_); +- fclose(seg_execution_time_fp_); ++ else if(policy.compare(std::string("SCHED_FIFO")) == 0){ ++ return set_sched_fifo(getpid(), attr.sched_priority, 99); + } +- +- exit(0); +-} +- +-void start_job(){ +- gpu_seg_id_ = 0; +- cpu_seg_id_ = 0; +- start_job_profiling(); +-} +- +-void finish_job(){ +- finish_job_profiling(cpu_seg_id_); +-} +- +-void request_gpu(){ +- if(is_task_ready_ != TASK_READY) return; +- if(was_in_loop_ == 1){ +- was_in_loop_ = 0; +- loop_cnt_ = 0; +- gpu_seg_cnt_in_loop_ = 0; ++ else if(policy.compare(std::string("SCHED_RR")) == 0){ ++ return set_sched_rr(getpid(), attr.sched_priority, 99); + } +- +- stop_profiling_cpu_seg_response_time(cpu_seg_id_, 1); +- if(gpu_scheduling_flag_==1){ +- unsigned long relative_deadline = gpu_deadline_list_[gpu_seg_id_]; +- +- if(gpu_seg_id_ > max_gpu_id_){ +- printf("[ERROR] %s - GPU segment id bigger than max segment id!\n", task_filename_.c_str()); +- printf("gpu seg id: %d / max seg id: %d\n", gpu_seg_id_); +- relative_deadline = 1000; // 1us +- } +- +- gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline; +- gpu_sched_info_->state = SCHEDULING_STATE_WAIT; ++ else if(policy.compare(std::string("SCHED_DEADLINE")) == 0){ ++ return set_sched_deadline(getpid(), attr.sched_runtime, attr.sched_deadline, attr.sched_period); + } +- +- start_profiling_gpu_seg_response_time(); +- +- #ifdef DEBUG +- printf("request_gpu: %d\n", gpu_seg_id_); +- #endif +- +- +- if(gpu_scheduling_flag_ == 1){ +- while(1){ +- kill(gpu_scheduler_pid_, SIGUSR1); +- if(gpu_sched_info_->scheduling_flag == 1) break; +- } ++ else{ ++ std::cout<<"[ERROR] Invalidate scheduling policy: "<state = SCHEDULING_STATE_RUN; +- gpu_sched_info_->deadline = -1; +- } ++ return true; + } + +-void request_gpu_in_loop(int flag){ +- if(is_task_ready_ != TASK_READY) return; +- was_in_loop_ = 1; ++void yield_task_scheduling(){ ++ sched_yield(); ++} + +- if(flag == GPU_SEG_LOOP_START){ +- loop_cnt_++; +- if(gpu_seg_cnt_in_loop_ == 0){ +- gpu_seg_cnt_in_loop_ = 1; +- } +- else{ +- gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_; +- cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_; +- gpu_seg_cnt_in_loop_ = 1; +- } ++void sig_handler(int signum){ ++ if(signum == SIGINT || signum == SIGTSTP || signum == SIGQUIT){ ++ termination(); + } ++} + +- if(flag != GPU_SEG_LOOP_START){ +- gpu_seg_cnt_in_loop_++; ++void termination(){ ++ printf("TERMINATION\n"); ++ if(remove(task_filename_.c_str())){ ++ printf("Cannot remove file %s\n", task_filename_); ++ exit(1); + } + +- stop_profiling_cpu_seg_response_time(cpu_seg_id_, loop_cnt_); +- if(gpu_scheduling_flag_==1){ +- unsigned long relative_deadline = gpu_deadline_list_[gpu_seg_id_]; +- +- if(gpu_seg_id_ > max_gpu_id_){ +- printf("[ERROR] %s - GPU segment id bigger than max segment id!\n", task_filename_.c_str()); +- relative_deadline = 1000; // 1us +- } +- +- gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline; +- gpu_sched_info_->state = SCHEDULING_STATE_WAIT; +- } ++ fclose(task_response_time_fp_); + +- start_profiling_gpu_seg_response_time(); +- +- #ifdef DEBUG +- printf("request_gpu: %d\n", gpu_seg_id_); +- #endif ++ exit(0); ++} + +- if(gpu_scheduling_flag_ == 1){ +- while(1){ +- kill(gpu_scheduler_pid_, SIGUSR1); +- if(gpu_sched_info_->scheduling_flag == 1) break; ++std::string get_cmd_output(const char* cmd) { ++ char buffer[128]; ++ std::string result = ""; ++ FILE* pipe = popen(cmd, "r"); ++ if (!pipe) throw std::runtime_error("popen() failed!"); ++ try { ++ while (fgets(buffer, sizeof(buffer), pipe) != NULL) { ++ result += buffer; ++ } ++ } catch (...) { ++ pclose(pipe); ++ throw; + } +- } +- +- start_profiling_gpu_seg_execution_time(); +- +- if(gpu_scheduling_flag_ == 1){ +- gpu_sched_info_->state = SCHEDULING_STATE_RUN; +- gpu_sched_info_->deadline = -1; +- } ++ pclose(pipe); ++ return result; + } + +-void yield_gpu(std::string remark){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- +- #ifdef DEBUG +- printf("yield_gpu: %d, %s\n", gpu_seg_id_, remark.c_str()); +- #endif ++std::vector get_child_pids(int pid){ ++ std::vector child_pids; ++ std::string cmd = "ps -ef -T | grep " + std::to_string(pid); ++ std::string s = get_cmd_output(cmd.c_str()); + +- stop_profiling_gpu_seg_time(gpu_seg_id_, 1, remark); +- start_profiling_cpu_seg_response_time(); +- gpu_seg_id_++; +- cpu_seg_id_++; +-} ++ std::vector task_ps_info_vec = tokenize_string(s, "\n"); + +-void yield_gpu_in_loop(int flag, std::string remark){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; ++ for(auto it = task_ps_info_vec.begin(); it != task_ps_info_vec.end(); ++it){ ++ std::string task_ps_info = *it; ++ if(task_ps_info.find(std::string("grep")) != std::string::npos) continue; ++ if(task_ps_info.find(std::string("ps -ef -T")) != std::string::npos) continue; ++ std::vector parsed_task_ps_info = tokenize_string(task_ps_info, " "); ++ int child_pid = std::stoi(parsed_task_ps_info[2]); ++ if(child_pid == pid) continue; ++ child_pids.push_back(child_pid); + } + +- #ifdef DEBUG +- printf("yield_gpu: %d\n", gpu_seg_id_); +- #endif +- +- stop_profiling_gpu_seg_time(gpu_seg_id_, loop_cnt_, remark); +- start_profiling_cpu_seg_response_time(); +- +- gpu_seg_id_++; +- cpu_seg_id_++; +- +- // if(flag == GPU_SEG_LOOP_END){ +- // gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1; +- // cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1; +- // } +-} +- +-void init_task(){ +- is_task_ready_ = TASK_READY; ++ return child_pids; + } + +-void disable_task(){ +- is_task_ready_ = TASK_NOT_READY; +-} ++std::vector tokenize_string(std::string s, std::string delimiter){ ++ std::vector output; ++ size_t pos = 0; + +-void print_loop_info(std::string tag){ +- std::cout<<"tag: "<pid = getpid(); +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- gpu_sched_info_->scheduling_flag = 0; +- printf("Task [%d] is ready to work\n", getpid()); +- +- gpu_seg_id_ = 0; +- cpu_seg_id_ = 0; +- +- return; +-} +- + void sig_handler(int signum){ + if(signum == SIGINT || signum == SIGTSTP || signum == SIGQUIT){ + termination(); + } + } + +-void get_deadline_list(){ +- char gpu_deadline_filename[rubis_lib_BUFFER_SIZE]; +- char* user_name = getenv("USER_HOME"); +- +- if(gpu_deadline_filename_[0] != '~'){ +- strcpy(gpu_deadline_filename, gpu_deadline_filename_); +- } +- else{ +- strcpy(gpu_deadline_filename, user_name); +- strcat(gpu_deadline_filename, &gpu_deadline_filename_[1]); +- } +- +- FILE* fp; +- fp = fopen(gpu_deadline_filename, "r"); +- if(fp==NULL){ +- fprintf(stderr, "Cannot find file %s\n", gpu_deadline_filename); +- exit(1); +- } +- +- char* buf; +- int cnt = 0; +- size_t len = 0; +- ssize_t n; +- +- getline(&buf, &len, fp); // skip first line +- while( (n = getline(&buf, &len, fp)) != -1 ){ +- cnt++; +- } +- max_gpu_id_ = cnt; +- gpu_deadline_list_ = (unsigned long *)malloc(sizeof(unsigned long) * max_gpu_id_); +- rewind(fp); +- +- int idx = 0; +- +- getline(&buf, &len, fp); // skip first line +- while((n = getline(&buf, &len, fp)) != -1){ +- unsigned long deadline; +- sscanf(buf, "gpu_%*d,%llu", &deadline); +- gpu_deadline_list_[idx++] = deadline; +- } +- +- free(buf); // free allocated memory at getline +- fclose(fp); +- +- // print_gpu_deadline_list(); +- +- return; +-} +- + void termination(){ + printf("TERMINATION\n"); +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->state = SCHEDULING_STATE_STOP; +- shmdt(gpu_sched_info_); +- } + +- free(gpu_deadline_list_); + if(remove(task_filename_)){ + printf("Cannot remove file %s\n", task_filename_); + exit(1); + } + +- if(task_profiling_flag_){ +- fclose(task_response_time_fp_); +- } +- +- if(gpu_profiling_flag_){ +- fclose(seg_response_time_fp_); +- fclose(seg_execution_time_fp_); +- free(task_filename_); +- free(gpu_deadline_filename_); +- } ++ fclose(task_response_time_fp_); + + exit(0); +-} +- +-void start_job(){ +- gpu_seg_id_ = 0; +- cpu_seg_id_ = 0; +- start_job_profiling(); +-} +- +-void finish_job(){ +- finish_job_profiling(cpu_seg_id_); +-} +- +-void request_gpu(){ +- if(is_task_ready_ != TASK_READY) return; +- if(was_in_loop_ == 1){ +- was_in_loop_ = 0; +- loop_cnt_ = 0; +- gpu_seg_cnt_in_loop_ = 0; +- } +- +- stop_profiling_cpu_seg_response_time(cpu_seg_id_, 1); +- if(gpu_scheduling_flag_==1){ +- unsigned long relative_deadline = gpu_deadline_list_[gpu_seg_id_]; +- +- if(gpu_seg_id_ > max_gpu_id_){ +- printf("[ERROR] %s - GPU segment id bigger than max segment id!\n", task_filename_); +- printf("gpu seg id: %d / max seg id: %d\n", gpu_seg_id_, max_gpu_id_); +- relative_deadline = 1000; // 1us +- } +- +- gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline; +- gpu_sched_info_->state = SCHEDULING_STATE_WAIT; +- } +- +- start_profiling_gpu_seg_response_time(); +- +- #ifdef DEBUG +- printf("request_gpu: %d\n", gpu_seg_id_); +- #endif +- +- if(gpu_scheduling_flag_ == 1){ +- while(1){ +- kill(gpu_scheduler_pid_, SIGUSR1); +- if(gpu_sched_info_->scheduling_flag == 1) break; +- } +- } +- +- start_profiling_gpu_seg_execution_time(); +- +- if(gpu_scheduling_flag_ == 1){ +- gpu_sched_info_->state = SCHEDULING_STATE_RUN; +- gpu_sched_info_->deadline = -1; +- } +-} +- +-void request_gpu_in_loop(int flag){ +- if(is_task_ready_ != TASK_READY) return; +- was_in_loop_ = 1; +- +- if(flag == GPU_SEG_LOOP_START){ +- loop_cnt_++; +- if(gpu_seg_cnt_in_loop_ == 0){ +- gpu_seg_cnt_in_loop_ = 1; +- } +- else{ +- gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_; +- cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_; +- gpu_seg_cnt_in_loop_ = 1; +- } +- } +- +- if(flag != GPU_SEG_LOOP_START){ +- gpu_seg_cnt_in_loop_++; +- } +- +- stop_profiling_cpu_seg_response_time(cpu_seg_id_, loop_cnt_); +- if(gpu_scheduling_flag_==1){ +- unsigned long relative_deadline = gpu_deadline_list_[gpu_seg_id_]; +- +- if(gpu_seg_id_ > max_gpu_id_){ +- printf("[ERROR] %s - GPU segment id bigger than max segment id!\n", task_filename_); +- relative_deadline = 1000; // 1us +- } +- +- gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline; +- gpu_sched_info_->state = SCHEDULING_STATE_WAIT; +- } +- +- start_profiling_gpu_seg_response_time(); +- +- #ifdef DEBUG +- printf("request_gpu: %d\n", gpu_seg_id_); +- #endif +- +- if(gpu_scheduling_flag_ == 1){ +- while(1){ +- kill(gpu_scheduler_pid_, SIGUSR1); +- if(gpu_sched_info_->scheduling_flag == 1) break; +- } +- } +- +- start_profiling_gpu_seg_execution_time(); +- +- if(gpu_scheduling_flag_ == 1){ +- gpu_sched_info_->state = SCHEDULING_STATE_RUN; +- gpu_sched_info_->deadline = -1; +- } +-} +- +-void yield_gpu(){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- +- #ifdef DEBUG +- printf("yield_gpu: %d\n", gpu_seg_id_); +- #endif +- +- stop_profiling_gpu_seg_time(gpu_seg_id_, 1); +- start_profiling_cpu_seg_response_time(1); +- +- gpu_seg_id_++; +- cpu_seg_id_++; +-} +- +-void yield_gpu_with_remark(const char* remark){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- +- #ifdef DEBUG +- printf("yield_gpu: %d\n", gpu_seg_id_); +- #endif +- +- stop_profiling_gpu_seg_time_with_remark(gpu_seg_id_, 1, remark); +- start_profiling_cpu_seg_response_time(); +- +- gpu_seg_id_++; +- cpu_seg_id_++; +-} +- +-void yield_gpu_in_loop(int flag){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- +- #ifdef DEBUG +- printf("yield_gpu: %d\n", gpu_seg_id_); +- #endif +- +- stop_profiling_gpu_seg_time(gpu_seg_id_, loop_cnt_); +- start_profiling_cpu_seg_response_time(); +- +- if(flag == GPU_SEG_LOOP_END){ +- gpu_seg_id_ -= gpu_seg_cnt_in_loop_; +- cpu_seg_id_ -= gpu_seg_cnt_in_loop_; +- } +-} +- +-void yield_gpu_with_remark_in_loop(int flag, const char* remark){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- +- #ifdef DEBUG +- printf("yield_gpu: %d\n", gpu_seg_id_); +- #endif +- +- stop_profiling_gpu_seg_time_with_remark(gpu_seg_id_, loop_cnt_, remark); +- start_profiling_cpu_seg_response_time(); +- +- // if(flag == GPU_SEG_LOOP_END){ +- // gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1; +- // cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1; +- // } +-} +- +-void init_task(){ +- is_task_ready_ = TASK_READY; +-} +- +-void disable_task(){ +- is_task_ready_ = TASK_NOT_READY; +-} +- +-void print_loop_info(const char* tag){ +- printf("tag: %s\n",tag); +- printf("cpu_seg_id: %d\n",cpu_seg_id_); +- printf("gpu_seg_id: %d\n",gpu_seg_id_); +- printf("loop cnt: %d\n",loop_cnt_); +- printf("loop seg cnt: %d\n",gpu_seg_cnt_in_loop_); +-} +- +-void print_gpu_deadline_list(){ +- printf("====================================\n[GPU deadline list]\n"); +- printf("gpu_id\tdeadline\n"); +- for(int i = 0; i < max_gpu_id_; i++){ +- printf("%d\t%lu\n",i,gpu_deadline_list_[i]); +- } +- printf("====================================\n"); +-} ++} +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c.backup b/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c.backup +deleted file mode 100644 +index 7f5bfb1d..00000000 +--- a/autoware.ai/src/autoware/common/rubis_lib/src/sched_c.c.backup ++++ /dev/null +@@ -1,370 +0,0 @@ +-#include "rubis_lib/sched_c.h" +- +-int key_id_; +-int is_scheduled_; +-int gpu_scheduling_flag_; +-GPUSchedInfo* gpu_sched_info_; +-int gpu_scheduler_pid_; +-char* task_filename_; +-char* gpu_deadline_filename_; +-// unsigned long long gpu_deadline_list_[1024]; +-unsigned long long* gpu_deadline_list_; +-unsigned int max_gpu_id_ = TASK_NOT_READY; +-unsigned int cpu_seg_id_ = 0; +-unsigned int gpu_seg_id_ = 0; +- +-int task_state_ = TASK_STATE_READY; +-int is_task_ready_ = 0; +-int was_in_loop_ = 0; +-int loop_cnt_ = 0; +-int gpu_seg_cnt_in_loop_ = 0; +- +- +-// system call hook to call SCHED_DEADLINE +-int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags){ +- return syscall(__NR_sched_setattr, pid, attr, flags); +-} +- +-int sched_getattr(pid_t pid, struct sched_attr *attr, unsigned int size, unsigned int flags) +-{ +- return syscall(__NR_sched_getattr, pid, attr, size, flags); +-} +- +-int set_sched_deadline(int _tid, __u64 _exec_time, __u64 _deadline, __u64 _period) { +- struct sched_attr attr; +- attr.size = sizeof(attr); +- attr.sched_flags = 0; +- attr.sched_nice = 0; +- attr.sched_priority = 0; +- +- attr.sched_policy = SCHED_DEADLINE; // 6 +- attr.sched_runtime = _exec_time; +- attr.sched_deadline = _deadline; +- attr.sched_period = _period; +- +- int ret = sched_setattr(_tid, &attr, attr.sched_flags); +- if(ret < 0) { +- printf("[ERROR] sched_setattr failed. Are you root? (%d)\n", ret); +- perror("sched_setattr"); +- exit(-1); +- return 0; +- } +- return 1; +-} +- +-void request_task_scheduling(double task_minimum_inter_release_time, double task_execution_time, double task_relative_deadline){ +- set_sched_deadline(gettid(), +- (u_int64_t)(task_execution_time), +- (u_int64_t)(task_relative_deadline), +- (u_int64_t)(task_minimum_inter_release_time) +- ); +-} +- +-void yield_task_scheduling(){ +- sched_yield(); +-} +- +-void init_gpu_scheduling(char* task_filename, char* gpu_deadline_filename, int key_id){ +- gpu_scheduling_flag_ = 1; +- +- task_filename_ = (char *)malloc(strlen(task_filename) * sizeof(char)); +- gpu_deadline_filename_ = (char *)malloc(strlen(gpu_deadline_filename) * sizeof(char)); +- +- strcpy(task_filename_, task_filename); +- strcpy(gpu_deadline_filename_, gpu_deadline_filename); +- key_id_ = key_id; +- +- // Get deadlines +- printf("deadline filename: %s\n", gpu_deadline_filename_); +- get_deadline_list(); +- +- // Init signal handler +- signal(SIGINT, sig_handler); +- signal(SIGTSTP, sig_handler); +- signal(SIGQUIT, sig_handler); +- +- // Create task file +- FILE* task_fp; +- task_fp = fopen(task_filename_, "w"); +- if(task_fp == NULL){ +- printf("Cannot create task file at %s\n", task_filename_); +- exit(1); +- } +- fprintf(task_fp, "%d\n", getpid()); +- fprintf(task_fp, "%d", key_id_); +- fclose(task_fp); +- +- // Get pid of scheduler +- FILE* scheduler_fp; +- printf("Wait the scheduler...\n"); +- +- while(1){ +- scheduler_fp = fopen("/tmp/np_edf_scheduler", "r"); +- if(scheduler_fp) break; +- } +- while(1){ +- fscanf(scheduler_fp, "%d", &gpu_scheduler_pid_); +- if(gpu_scheduler_pid_ != 0) break; +- } +- printf("Scheduler pid: %d\n", gpu_scheduler_pid_); +- fclose(scheduler_fp); +- +- +- // Initialize scheduling information (shared memory data) +- FILE* sm_key_fp; +- sm_key_fp = fopen("/tmp/sm_key", "r"); +- if(sm_key_fp == NULL){ +- printf("Cannot open /tmp/sm_key\n"); +- termination(); +- } +- +- key_t key; +- int shmid; +- key = ftok("/tmp/sm_key", key_id_); +- shmid = shmget(key, sizeof(GPUSchedInfo), 0666|IPC_CREAT); +- gpu_sched_info_ = (GPUSchedInfo*)shmat(shmid, 0, 0); +- gpu_sched_info_->pid = getpid(); +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- gpu_sched_info_->scheduling_flag = 0; +- printf("Task [%d] is ready to work\n", getpid()); +- +- gpu_seg_id_ = 0; +- cpu_seg_id_ = 0; +- +- return; +-} +- +-void sig_handler(int signum){ +- if(signum == SIGINT || signum == SIGTSTP || signum == SIGQUIT){ +- termination(); +- } +-} +- +-void get_deadline_list(){ +- char gpu_deadline_filename[rubis_lib_BUFFER_SIZE]; +- char* user_name = getenv("USER_HOME"); +- +- if(gpu_deadline_filename_[0] != '~'){ +- strcpy(gpu_deadline_filename, gpu_deadline_filename_); +- } +- else{ +- strcpy(gpu_deadline_filename, user_name); +- strcat(gpu_deadline_filename, &gpu_deadline_filename_[1]); +- } +- +- FILE* fp; +- fp = fopen(gpu_deadline_filename, "r"); +- if(fp==NULL){ +- fprintf(stderr, "Cannot find file %s\n", gpu_deadline_filename); +- exit(1); +- } +- char buf[1024]; +- +- while(1){ +- int id; +- if(!fgets(buf, 1024, fp)) break; +- strtok(buf, "\n"); +- sscanf(buf, "%d, %*llu", &id); +- if(gpu_seg_id_ > max_gpu_id_) max_gpu_id_ = id; +- } +- +- gpu_deadline_list_ = (unsigned long long *)malloc(sizeof(unsigned long long) * (max_gpu_id_+1)); +- printf("file read is finished\n"); +- +- rewind(fp); +- while(1){ +- int id; +- long long int deadline; +- if(!fgets(buf, 1024, fp)) break; +- sscanf(buf, "%d, %llu", &id, &deadline); +- gpu_deadline_list_[id] = deadline; +- } +- fclose(fp); +- +- return; +-} +- +-void termination(){ +- printf("TERMINATION\n"); +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->state = SCHEDULING_STATE_STOP; +- shmdt(gpu_sched_info_); +- } +- +- free(gpu_deadline_list_); +- if(remove(task_filename_)){ +- printf("Cannot remove file %s\n", task_filename_); +- exit(1); +- } +- +- if(task_profiling_flag_){ +- fclose(task_response_time_fp_); +- } +- +- if(gpu_profiling_flag_){ +- fclose(seg_response_time_fp_); +- fclose(seg_execution_time_fp_); +- free(task_filename_); +- free(gpu_deadline_filename_); +- } +- +- exit(0); +-} +- +-void start_job(){ +- gpu_seg_id_ = 0; +- cpu_seg_id_ = 0; +- start_job_profiling(); +-} +- +-void finish_job(){ +- finish_job_profiling(cpu_seg_id_); +-} +- +-void request_gpu(){ +- if(was_in_loop_ == 1){ +- was_in_loop_ = 0; +- loop_cnt_ = 0; +- gpu_seg_cnt_in_loop_ = 0; +- } +- +- stop_profiling_cpu_seg_response_time(cpu_seg_id_, 1); +- if(gpu_scheduling_flag_==1){ +- if(gpu_seg_id_ > max_gpu_id_){ +- printf("[ERROR] GPU segment id bigger than max segment id!\n"); +- exit(1); +- } +- +- unsigned long long relative_deadline = gpu_deadline_list_[gpu_seg_id_]; +- gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline; +- gpu_sched_info_->state = SCHEDULING_STATE_WAIT; +- } +- +- start_profiling_gpu_seg_response_time(); +- +- if(gpu_scheduling_flag_ == 1){ +- while(1){ +- kill(gpu_scheduler_pid_, SIGUSR1); +- if(gpu_sched_info_->scheduling_flag == 1) break; +- } +- } +- +- start_profiling_gpu_seg_execution_time(); +- +- if(gpu_scheduling_flag_ == 1){ +- gpu_sched_info_->state = SCHEDULING_STATE_RUN; +- gpu_sched_info_->deadline = -1; +- } +-} +- +-void request_gpu_in_loop(int flag){ +- was_in_loop_ = 1; +- +- if(flag == GPU_SEG_LOOP_START){ +- loop_cnt_++; +- if(gpu_seg_cnt_in_loop_ == 0){ +- gpu_seg_cnt_in_loop_ = 1; +- } +- else{ +- gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_; +- cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_; +- gpu_seg_cnt_in_loop_ = 1; +- } +- } +- +- if(flag != GPU_SEG_LOOP_START){ +- gpu_seg_cnt_in_loop_++; +- } +- +- stop_profiling_cpu_seg_response_time(cpu_seg_id_, loop_cnt_); +- if(gpu_scheduling_flag_==1){ +- if(gpu_seg_id_ > max_gpu_id_){ +- printf("[ERROR] GPU segment id bigger than max segment id!\n"); +- exit(1); +- } +- +- unsigned long long relative_deadline = gpu_deadline_list_[gpu_seg_id_]; +- gpu_sched_info_->deadline = get_current_time_ns() + relative_deadline; +- gpu_sched_info_->state = SCHEDULING_STATE_WAIT; +- } +- +- start_profiling_gpu_seg_response_time(); +- +- if(gpu_scheduling_flag_ == 1){ +- while(1){ +- kill(gpu_scheduler_pid_, SIGUSR1); +- if(gpu_sched_info_->scheduling_flag == 1) break; +- } +- } +- +- start_profiling_gpu_seg_execution_time(); +- +- if(gpu_scheduling_flag_ == 1){ +- gpu_sched_info_->state = SCHEDULING_STATE_RUN; +- gpu_sched_info_->deadline = -1; +- } +-} +- +-void yield_gpu(){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- stop_profiling_gpu_seg_time(gpu_seg_id_, 1); +- start_profiling_cpu_seg_response_time(1); +- +- gpu_seg_id_++; +- cpu_seg_id_++; +-} +- +-void yield_gpu_with_remark(const char* remark){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- stop_profiling_gpu_seg_time_with_remark(gpu_seg_id_, 1, remark); +- start_profiling_cpu_seg_response_time(); +- +- gpu_seg_id_++; +- cpu_seg_id_++; +-} +- +-void yield_gpu_in_loop(int flag){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- stop_profiling_gpu_seg_time(gpu_seg_id_, loop_cnt_); +- start_profiling_cpu_seg_response_time(); +- +- if(flag == GPU_SEG_LOOP_END){ +- gpu_seg_id_ -= gpu_seg_cnt_in_loop_; +- cpu_seg_id_ -= gpu_seg_cnt_in_loop_; +- } +-} +- +-void yield_gpu_with_remark_in_loop(int flag, const char* remark){ +- if(gpu_scheduling_flag_==1){ +- gpu_sched_info_->scheduling_flag = 0; +- gpu_sched_info_->state = SCHEDULING_STATE_NONE; +- } +- stop_profiling_gpu_seg_time_with_remark(gpu_seg_id_, loop_cnt_, remark); +- start_profiling_cpu_seg_response_time(); +- +- // if(flag == GPU_SEG_LOOP_END){ +- // gpu_seg_id_ = gpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1; +- // cpu_seg_id_ = cpu_seg_id_ - gpu_seg_cnt_in_loop_ + 1; +- // } +-} +- +-void init_task(){ +- is_task_ready_ = TASK_READY; +-} +- +-void print_loop_info(const char* tag){ +- printf("tag: %s\n",tag); +- printf("cpu_seg_id: %d\n",cpu_seg_id_); +- printf("gpu_seg_id: %d\n",gpu_seg_id_); +- printf("loop cnt: %d\n",loop_cnt_); +- printf("loop seg cnt: %d\n",gpu_seg_cnt_in_loop_); +-} +diff --git a/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling.cpp b/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling.cpp +index 0be0a0b6..211b9c99 100644 +--- a/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling.cpp ++++ b/autoware.ai/src/autoware/common/rubis_lib/src/sched_profiling.cpp +@@ -1,29 +1,15 @@ + #include "rubis_lib/sched_profiling.hpp" + + namespace rubis{ +- int instance_mode_; + unsigned long instance_; +-namespace sched{ +- int task_profiling_flag_; +- int gpu_profiling_flag_; ++ unsigned long obj_instance_; ++ + int iter_; + + FILE* task_response_time_fp_; +- FILE* seg_execution_time_fp_; +- FILE* seg_response_time_fp_; +- +- int is_gpu_profiling_started_; + + struct timespec task_start_time_; + struct timespec task_end_time_; +- unsigned long gpu_seg_response_time_; +- unsigned long gpu_seg_execution_time_; +- unsigned long cpu_seg_response_time_; +- int is_gpu_profiling_ready_ = 0; +- +- void start_gpu_profiling(){ +- is_gpu_profiling_ready_ = 1; +- } + + void init_task_profiling(std::string task_reponse_time_filename){ + if(task_reponse_time_filename.at(0) == '~'){ +@@ -32,7 +18,6 @@ namespace sched{ + task_reponse_time_filename = user_home_str + task_reponse_time_filename; + } + +- task_profiling_flag_ = 1; + task_response_time_fp_ = fopen(task_reponse_time_filename.c_str(), "w+"); + if(task_response_time_fp_ == NULL){ + std::cout<<"Cannot create/open file: "< dev_ptr(input); +- rubis::sched::request_gpu(); + thrust::exclusive_scan(dev_ptr, dev_ptr + ele_num, dev_ptr); +- rubis::sched::yield_gpu("71_exclusive_scan"); + checkCudaErrors(cudaDeviceSynchronize()); + + *sum = *(dev_ptr + ele_num - 1); +@@ -408,65 +392,43 @@ + int *cluster_offset; + int cluster_num, old_cluster_num; + +- rubis::sched::request_gpu(); + pclEuclideanInitialize << < grid_x, block_x >> > (cluster_indices_, size_); +- rubis::sched::yield_gpu("8_pclEuclideanInitialize"); + checkCudaErrors(cudaDeviceSynchronize()); + + old_cluster_num = cluster_num = size_; + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&cluster_offset, (size_ + 1) * sizeof(int))); +- rubis::sched::yield_gpu("9_cudaMalloc"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemset(cluster_offset, 0, (size_ + 1) * sizeof(int))); +- rubis::sched::yield_gpu("10_cudaMemset"); + +- rubis::sched::request_gpu(); + blockLabelling << < grid_x, block_x >> > (x_, y_, z_, cluster_indices_, size_, threshold_); +- rubis::sched::yield_gpu("11_blockLabelling"); + +- rubis::sched::request_gpu(); + clusterMark << < grid_x, block_x >> > (cluster_indices_, cluster_offset, size_); +- rubis::sched::yield_gpu("12_clusterMark"); + + + exclusiveScan(cluster_offset, size_ + 1, &cluster_num); + + int *cluster_list, *new_cluster_list, *tmp; + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&cluster_list, cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("13_cudaMalloc"); + +- rubis::sched::request_gpu(); + clusterCollector << < grid_x, block_x >> > (cluster_indices_, cluster_list, cluster_offset, size_); +- rubis::sched::yield_gpu("14_clusterCollector"); + + checkCudaErrors(cudaDeviceSynchronize()); + + int *cluster_matrix; + int *new_cluster_matrix; + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&cluster_matrix, cluster_num * cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("15_cudaMalloc"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemset(cluster_matrix, 0, cluster_num * cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("16_cudaMemset"); + + checkCudaErrors(cudaDeviceSynchronize()); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&new_cluster_list, cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("17_cudaMalloc"); + +- rubis::sched::request_gpu(); + buildClusterMatrix << < grid_x, block_x >> > + (x_, y_, z_, cluster_indices_, cluster_matrix, cluster_offset, size_, cluster_num, threshold_); +- rubis::sched::yield_gpu("18_buildClusterMatrix"); + checkCudaErrors(cudaDeviceSynchronize()); + + int block_x2 = 0, grid_x2 = 0; +@@ -479,51 +441,33 @@ + block_x2 = (cluster_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : cluster_num; + grid_x2 = (cluster_num - 1) / block_x2 + 1; + +- rubis::sched::request_gpu(); + mergeClusters << < grid_x2, block_x2 >> > (cluster_matrix, cluster_list, cluster_num); +- rubis::sched::yield_gpu("19_mergeClusters"); + +- rubis::sched::request_gpu(); + reflexClusterChanges << < grid_x, block_x >> > (cluster_indices_, cluster_offset, cluster_list, size_); +- rubis::sched::yield_gpu("20_reflexClusterChanges"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemset(cluster_offset, 0, (size_ + 1) * sizeof(int))); +- rubis::sched::yield_gpu("21_cudaMemset"); + +- rubis::sched::request_gpu(); + clusterMark << < grid_x2, block_x2 >> > (cluster_list, cluster_offset, cluster_num); +- rubis::sched::yield_gpu("22_clusterMark"); + + exclusiveScan(cluster_offset, size_ + 1, &cluster_num); + + if (grid_x2 == 1 && cluster_num == old_cluster_num) + break; + +- rubis::sched::request_gpu(); + clusterCollector << < grid_x2, block_x2 >> > (cluster_list, new_cluster_list, cluster_offset, old_cluster_num); +- rubis::sched::yield_gpu("23_clusterCollector"); + + checkCudaErrors(cudaDeviceSynchronize()); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&new_cluster_matrix, cluster_num * cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("24_cudaMalloc"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemset(new_cluster_matrix, 0, cluster_num * cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("25_cudaMemset"); + +- rubis::sched::request_gpu(); + rebuildClusterMatrix << < grid_x2, block_x2 >> > + (cluster_matrix, cluster_list, new_cluster_matrix, cluster_offset, old_cluster_num, cluster_num); +- rubis::sched::yield_gpu("26_rebuildClusterMatrix"); + + checkCudaErrors(cudaDeviceSynchronize()); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_matrix)); +- rubis::sched::yield_gpu("27_free"); + + cluster_matrix = new_cluster_matrix; + tmp = cluster_list; +@@ -533,32 +477,19 @@ + + cluster_num_ = cluster_num; + +- rubis::sched::request_gpu(); + resetClusterIndexes << < grid_x, block_x >> > (cluster_indices_, cluster_offset, size_); +- rubis::sched::yield_gpu("28_resetClusterIndexes"); + + checkCudaErrors(cudaDeviceSynchronize()); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemcpy(cluster_indices_host_, cluster_indices_, size_ * sizeof(int), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("29_dtoh"); + +- +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_matrix)); +- rubis::sched::yield_gpu("30_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_list)); +- rubis::sched::yield_gpu("31_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(new_cluster_list)); +- rubis::sched::yield_gpu("32_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_offset)); +- rubis::sched::yield_gpu("33_free"); + } + + extern "C" __global__ void mergeSelfClusters(int *cluster_matrix, int *cluster_list, int cluster_num, bool *changed) +@@ -728,80 +659,53 @@ + int *cluster_offset; + int cluster_num, old_cluster_num; + +- rubis::sched::request_gpu(); + pclEuclideanInitialize << < grid_x, block_x >> > (cluster_indices_, size_); +- rubis::sched::yield_gpu("34_pclEuclideanInitialize"); + + checkCudaErrors(cudaDeviceSynchronize()); + + old_cluster_num = cluster_num = size_; + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&cluster_offset, (size_ + 1) * sizeof(int))); +- rubis::sched::yield_gpu("35_cudaMalloc"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemset(cluster_offset, 0, (size_ + 1) * sizeof(int))); +- rubis::sched::yield_gpu("36_cudaMemset"); + +- rubis::sched::request_gpu(); + blockLabelling << < grid_x, block_x >> > (x_, y_, z_, cluster_indices_, size_, threshold_); +- rubis::sched::yield_gpu("37_blockLabelling"); + +- rubis::sched::request_gpu(); + clusterMark << < grid_x, block_x >> > (cluster_indices_, cluster_offset, size_); +- rubis::sched::yield_gpu("38_clusterMark"); +- + + exclusiveScan(cluster_offset, size_ + 1, &cluster_num); + + int *cluster_list, *new_cluster_list, *tmp; + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&cluster_list, cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("39_cudaMalloc"); + +- rubis::sched::request_gpu(); + clusterCollector << < grid_x, block_x >> > (cluster_indices_, cluster_list, cluster_offset, size_); +- rubis::sched::yield_gpu("40_clusterCollector"); + checkCudaErrors(cudaDeviceSynchronize()); + + int *cluster_matrix; + int *new_cluster_matrix; + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&cluster_matrix, cluster_num * cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("41_cudaMalloc"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemset(cluster_matrix, 0, cluster_num * cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("42_cudaMemset"); + + checkCudaErrors(cudaDeviceSynchronize()); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&new_cluster_list, cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("43_cudaMalloc"); + +- rubis::sched::request_gpu(); + buildClusterMatrix << < grid_x, block_x >> > + (x_, y_, z_, cluster_indices_, cluster_matrix, cluster_offset, size_, cluster_num, threshold_); +- rubis::sched::yield_gpu("44_buildClusterMatrix"); + checkCudaErrors(cudaDeviceSynchronize()); + + int block_x2 = 0, grid_x2 = 0; + + bool *changed; + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMallocHost(&changed, sizeof(bool))); +- rubis::sched::yield_gpu("45_cudaMallocHost"); + + #ifndef SERIAL + int *changed_diag; +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMallocHost(&changed_diag, sizeof(int))); +- rubis::sched::yield_gpu("46_cudaMallocHost"); + #endif + + int max_base_row = 0; +@@ -812,9 +716,7 @@ + block_x2 = (cluster_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : cluster_num; + grid_x2 = (cluster_num - 1) / block_x2 + 1; + +- rubis::sched::request_gpu(); + mergeSelfClusters << < grid_x2, block_x2 >> > (cluster_matrix, cluster_list, cluster_num, changed); +- rubis::sched::yield_gpu("47_mergeSelfClusters"); + + checkCudaErrors(cudaDeviceSynchronize()); + +@@ -837,14 +739,12 @@ + #ifdef SERIAL + //Merge clusters in each sub-matrix by moving from top to bottom of the similarity sub-matrix + for (int shift_level = 0; !(*changed) && shift_level < sub_matrix_col; shift_level++) { +- rubis::sched::request_gpu(); + mergeInterClusters<<>>(cluster_matrix, cluster_list, + shift_level, + base_row, base_column, + sub_matrix_row, sub_matrix_col, + sub_matrix_offset_row, sub_matrix_offset_col, + cluster_num, changed); +- rubis::sched::yield_gpu("48_mergeInterClusters"); + checkCudaErrors(cudaDeviceSynchronize()); + } + #else +@@ -855,13 +755,11 @@ + + *changed_diag = -1; + +- rubis::sched::request_gpu(); + clustersIntersecCheck << < grid_size, block_size >> > (cluster_matrix, changed_diag, + base_row, base_column, + sub_matrix_row, sub_matrix_col, + sub_matrix_offset_row, sub_matrix_offset_col, + cluster_num); +- rubis::sched::yield_gpu("49_clustersIntersecCheck"); + + checkCudaErrors(cudaDeviceSynchronize()); + +@@ -869,13 +767,11 @@ + { + //Merge clusters in sub-matrix that stay in the changed_diag diagonal by moving from top to bottom of the matrix. + +- rubis::sched::request_gpu(); + mergeInterClusters << < grid_x2, block_x2 >> > (cluster_matrix, cluster_list, *changed_diag, + base_row, base_column, + sub_matrix_row, sub_matrix_col, + sub_matrix_offset_row, sub_matrix_offset_col, + cluster_num, changed); +- rubis::sched::yield_gpu("50_mergeInterClusters"); + checkCudaErrors(cudaDeviceSynchronize()); + } + +@@ -891,48 +787,32 @@ + + if (*changed) + { +- rubis::sched::request_gpu(); + reflexClusterChanges << < grid_x, block_x >> > (cluster_indices_, cluster_offset, cluster_list, size_); +- rubis::sched::yield_gpu("51_reflexClusterChanges"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemset(cluster_offset, 0, (size_ + 1) * sizeof(int))); +- rubis::sched::yield_gpu("52_cudaMemset"); + + block_x2 = (cluster_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : cluster_num; + grid_x2 = (cluster_num - 1) / block_x2 + 1; + +- rubis::sched::request_gpu(); + clusterMark << < grid_x2, block_x2 >> > (cluster_list, cluster_offset, cluster_num); +- rubis::sched::yield_gpu("53_clusterMark"); + + old_cluster_num = cluster_num; + + exclusiveScan(cluster_offset, size_ + 1, &cluster_num); + +- rubis::sched::request_gpu(); + clusterCollector << < grid_x2, block_x2 >> > (cluster_list, new_cluster_list, cluster_offset, old_cluster_num); +- rubis::sched::yield_gpu("54_clusterCollector"); + + checkCudaErrors(cudaDeviceSynchronize()); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMalloc(&new_cluster_matrix, cluster_num * cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("55_cudaMalloc"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemset(new_cluster_matrix, 0, cluster_num * cluster_num * sizeof(int))); +- rubis::sched::yield_gpu("56_cudaMemset"); + +- rubis::sched::request_gpu(); + rebuildClusterMatrix << < grid_x2, block_x2 >> > + (cluster_matrix, cluster_list, new_cluster_matrix, cluster_offset, old_cluster_num, cluster_num); +- rubis::sched::yield_gpu("57_rebuildClusterMatrix"); + checkCudaErrors(cudaDeviceSynchronize()); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_matrix)); +- rubis::sched::yield_gpu("58_cudaFree"); + + cluster_matrix = new_cluster_matrix; + tmp = cluster_list; +@@ -945,39 +825,23 @@ + + //Reset all cluster indexes to make them start from 0 + +- rubis::sched::request_gpu(); + resetClusterIndexes << < grid_x, block_x >> > (cluster_indices_, cluster_offset, size_); +- rubis::sched::yield_gpu("59_resetClusterIndexes"); + + checkCudaErrors(cudaDeviceSynchronize()); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaMemcpy(cluster_indices_host_, cluster_indices_, size_ * sizeof(int), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("60_cudaMemcpy"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_matrix)); +- rubis::sched::yield_gpu("61_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_list)); +- rubis::sched::yield_gpu("62_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(new_cluster_list)); +- rubis::sched::yield_gpu("63_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_offset)); +- rubis::sched::yield_gpu("64_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFreeHost(changed)); +- rubis::sched::yield_gpu("65_free"); + #ifndef SERIAL +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFreeHost(changed_diag)); +- rubis::sched::yield_gpu("66_free"); + #endif + } + +@@ -1043,21 +907,13 @@ + + GpuEuclideanCluster::~GpuEuclideanCluster() + { +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(x_)); +- rubis::sched::yield_gpu("67_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(y_)); +- rubis::sched::yield_gpu("68_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(z_)); +- rubis::sched::yield_gpu("69_free"); + +- rubis::sched::request_gpu(); + checkCudaErrors(cudaFree(cluster_indices_)); +- rubis::sched::yield_gpu("70_free"); + + free(cluster_indices_host_); + } +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp +index 30aa9107..e157441c 100644 +--- a/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp ++++ b/autoware.ai/src/autoware/core_perception/lidar_euclidean_cluster_detect/nodes/lidar_euclidean_cluster_detect/lidar_euclidean_cluster_detect.cpp +@@ -229,16 +229,11 @@ void publishDetectedObjects(const autoware_msgs::CloudClusterArray &in_clusters) + _pub_detected_objects.publish(detected_objects); + + rubis_detected_objects.instance = rubis::instance_; ++ rubis_detected_objects.instance = rubis::obj_instance_; + rubis_detected_objects.object_array = detected_objects; + + _pub_rubis_detected_objects.publish(rubis_detected_objects); + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY){ +- rubis::sched::init_task(); +- if(rubis::sched::gpu_profiling_flag_) rubis::sched::start_gpu_profiling(); +- } +- +- rubis::sched::task_state_ = TASK_STATE_DONE; + } + + void publishCloudClusters(const ros::Publisher *in_publisher, const autoware_msgs::CloudClusterArray &in_clusters, +@@ -855,7 +850,9 @@ void removePointsUpTo(const pcl::PointCloud::Ptr in_cloud_ptr, + + void velodyne_callback(const rubis_msgs::PointCloud2ConstPtr& in_sensor_cloud) + { ++ rubis::start_task_profiling(); + rubis::instance_ = in_sensor_cloud->instance; ++ rubis::obj_instance_ = in_sensor_cloud->instance; + //_start = std::chrono::system_clock::now(); + if (!_using_sensor_cloud) + { +@@ -931,7 +928,10 @@ void velodyne_callback(const rubis_msgs::PointCloud2ConstPtr& in_sensor_cloud) + + _using_sensor_cloud = false; + } ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); + } ++ + int main(int argc, char **argv) + { + // Initialize ROS +@@ -940,33 +940,27 @@ int main(int argc, char **argv) + ros::NodeHandle h; + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"); ++ + int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- +- int gpu_scheduling_flag; +- int gpu_profiling_flag; +- std::string gpu_execution_time_filename; +- std::string gpu_response_time_filename; +- std::string gpu_deadline_filename; +- +- private_nh.param("/lidar_euclidean_cluster_detect/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/lidar_euclidean_cluster_detect/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/lidar_euclidean_cluster_detect/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv"); +- private_nh.param("/lidar_euclidean_cluster_detect/rate", rate, 10); +- private_nh.param("/lidar_euclidean_cluster_detect/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/lidar_euclidean_cluster_detect/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/lidar_euclidean_cluster_detect/task_relative_deadline", task_relative_deadline, (double)10); +- private_nh.param("/lidar_euclidean_cluster_detect/gpu_scheduling_flag", gpu_scheduling_flag, 0); +- private_nh.param("/lidar_euclidean_cluster_detect/gpu_profiling_flag", gpu_profiling_flag, 0); +- private_nh.param("/lidar_euclidean_cluster_detect/gpu_execution_time_filename", gpu_execution_time_filename, "~/Documents/gpu_profiling/test_clustering_execution_time.csv"); +- private_nh.param("/lidar_euclidean_cluster_detect/gpu_response_time_filename", gpu_response_time_filename, "~/Documents/gpu_profiling/test_clustering_response_time.csv"); +- private_nh.param("/lidar_euclidean_cluster_detect/gpu_deadline_filename", gpu_deadline_filename, "~/Documents/gpu_deadline/cluster_gpu_deadline.csv"); ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); + + tf::StampedTransform transform; + tf::TransformListener listener; +@@ -978,8 +972,6 @@ int main(int argc, char **argv) + + generateColors(_colors, 255); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- + std::string label; + std::string output_lane_str; + std::string output_cluster_str; +@@ -1111,40 +1103,8 @@ int main(int argc, char **argv) + + // Create a ROS subscriber for the input point cloud + ros::Subscriber sub = h.subscribe(points_topic, 1, velodyne_callback); +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- ros::spinOnce(); +- r.sleep(); +- if(rubis::sched::is_task_ready_) break; +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::start_job(); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::finish_job(); +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ ++ ros::spin(); + + return 0; + } +diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp +index e2e69cef..fbb518e6 100644 +--- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp ++++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda.cpp +@@ -107,8 +107,6 @@ void ImmUkfPda::rubis_callback(const rubis_msgs::DetectedObjectArray& input) + dumpResultText(detected_objects_output); + } + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; + } + + void ImmUkfPda::callback(const autoware_msgs::DetectedObjectArray& input) +diff --git a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp +index 8d99fbef..b8e03bb6 100644 +--- a/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp ++++ b/autoware.ai/src/autoware/core_perception/lidar_imm_ukf_pda_track/nodes/imm_ukf_pda/imm_ukf_pda_main.cpp +@@ -24,59 +24,21 @@ int main(int argc, char** argv) + ros::NodeHandle private_nh("~"); + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; + int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- +- private_nh.param("/imm_ukf_pda_track/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/imm_ukf_pda_track/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/imm_ukf_pda_track/task_response_time_filename", task_response_time_filename, "~/profiling/response_time/imm_ukf_pda_track.csv"); + private_nh.param("/imm_ukf_pda_track/rate", rate, 10); +- private_nh.param("/imm_ukf_pda_track/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/imm_ukf_pda_track/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/imm_ukf_pda_track/task_relative_deadline", task_relative_deadline, (double)10); + + ImmUkfPda app; + app.run(); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ ros::Rate r(rate); ++ while(ros::ok()){ ++ rubis::start_task_profiling(); + +- ros::spinOnce(); ++ ros::spinOnce(); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); ++ rubis::stop_task_profiling(rubis::instance_, 0); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ r.sleep(); + } + + return 0; +diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch +index b0642a01..1acf2d1d 100755 +--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch ++++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching.launch +@@ -20,7 +20,6 @@ + + + +- + + + +@@ -45,7 +44,6 @@ + + + +- + + + +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch +index ebf098ff..924a5183 100755 +--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch ++++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/launch/ndt_matching_params.launch +@@ -20,7 +20,6 @@ + + + +- + + + +@@ -45,7 +44,6 @@ + + + +- + + + +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/modular_ndt_matching/modular_ndt_matching.cpp b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/modular_ndt_matching/modular_ndt_matching.cpp +index c97357f1..052a6ebe 100644 +--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/modular_ndt_matching/modular_ndt_matching.cpp ++++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/modular_ndt_matching/modular_ndt_matching.cpp +@@ -846,13 +846,11 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) + + health_checker_ptr_->CHECK_RATE("topic_rate_ndt_pose_slow", 8, 5, 1, "topic ndt_pose publish rate slow."); + ndt_pose_pub.publish(ndt_pose_msg); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_ndt_pose_msg.instance = rubis::instance_; +- rubis_ndt_pose_msg.msg = ndt_pose_msg; +- rubis_ndt_pose_pub.publish(rubis_ndt_pose_msg); +- } ++ rubis_ndt_pose_msg.instance = rubis::instance_; ++ rubis_ndt_pose_msg.msg = ndt_pose_msg; ++ rubis_ndt_pose_pub.publish(rubis_ndt_pose_msg); + + localizer_pose_pub.publish(localizer_pose_msg); + +@@ -945,14 +943,10 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) + previous_velocity_z = current_velocity_z; + previous_accel = current_accel; + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY){ +- rubis::sched::init_task(); +- if(rubis::sched::gpu_profiling_flag_) rubis::sched::start_gpu_profiling(); +- } + } + + static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input){ +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + ndt_matching(input); + } + +@@ -1064,46 +1058,21 @@ int main(int argc, char** argv) + #endif + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; + double task_execution_time; + double task_relative_deadline; + +- int gpu_scheduling_flag; +- int gpu_profiling_flag; +- std::string gpu_execution_time_filename; +- std::string gpu_response_time_filename; +- std::string gpu_deadline_filename; +- + std::string node_name = ros::this_node::getName(); +- private_nh.param(node_name+"/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param(node_name+"/task_profiling_flag", task_profiling_flag, 0); + private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ndt_matching.csv"); + private_nh.param(node_name+"/rate", rate, 10); + private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10); + private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); +- private_nh.param(node_name+"/gpu_scheduling_flag", gpu_scheduling_flag, 0); +- private_nh.param(node_name+"/gpu_profiling_flag", gpu_profiling_flag, 0); +- private_nh.param(node_name+"/gpu_execution_time_filename", gpu_execution_time_filename, "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"); +- private_nh.param(node_name+"/gpu_response_time_filename", gpu_response_time_filename, "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"); +- private_nh.param(node_name+"/gpu_deadline_filename", gpu_deadline_filename, "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"); +- private_nh.param(node_name+"/instance_mode", rubis::instance_mode_, 0); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- if(gpu_profiling_flag) rubis::sched::init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + +- if( (_method_type == MethodType::PCL_ANH_GPU) && (gpu_scheduling_flag == 1) ){ +- rubis::sched::init_gpu_scheduling("/tmp/ndt_matching", gpu_deadline_filename, 0); +- } +- else if(_method_type != MethodType::PCL_ANH_GPU && gpu_scheduling_flag == 1){ +- ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!"); +- exit(1); +- } +- + Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation + Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation + Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY()); +@@ -1113,11 +1082,10 @@ int main(int argc, char** argv) + // Publishers + ndt_pose_pub = nh.advertise(_output_pose_topic, 10); + +- // if(rubis::instance_mode_) rubis_ndt_pose_pub = nh.advertise("/rubis_" + _output_pose_topic,10); + + //debug + std::string _output_pose_topic_rubis = "/rubis_" + _output_pose_topic; +- if(rubis::instance_mode_) rubis_ndt_pose_pub = nh.advertise(_output_pose_topic_rubis,10); ++ rubis_ndt_pose_pub = nh.advertise(_output_pose_topic_rubis,10); + + // localizer_pose_pub = nh.advertise("/localizer_pose", 10); + estimate_twist_pub = nh.advertise(_twist_topic, 10); +@@ -1131,13 +1099,11 @@ int main(int argc, char** argv) + ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 10, initialpose_callback); + + ros::Subscriber points_sub; +- // if(rubis::instance_mode_) points_sub = nh.subscribe("rubis_" + _input_topic, _queue_size, rubis_points_callback); + // else points_sub = nh.subscribe(_input_topic, _queue_size, points_callback); + + //debug + std::string _input_topic_rubis = "/rubis_" + _input_topic; +- if(rubis::instance_mode_) points_sub = nh.subscribe(_input_topic_rubis, _queue_size, rubis_points_callback); +- else points_sub = nh.subscribe(_input_topic, _queue_size, points_callback); ++ points_sub = nh.subscribe(_input_topic_rubis, _queue_size, rubis_points_callback); + + ros::Subscriber ins_stat_sub = nh.subscribe("/ins_stat", 1, ins_stat_callback); + +@@ -1149,45 +1115,28 @@ int main(int argc, char** argv) + std::cout<<"----------------modular_ndt_matching_debug start------------------- "<< std::endl; + std::cout<<"output pose topic: " << _output_pose_topic << std::endl; + std::cout<<"output pose topic rubis: " << _output_pose_topic_rubis << std::endl; +- std::cout<<"instance_mode param: " << std::endl; + std::cout<<"----------------modular_ndt_matching_debug end--------------------- "<< std::endl; +- // SPIN +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ ++ + ros::Rate r(rate); + +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(map_loaded == 1) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- map_sub.shutdown(); +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::start_job(); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ // Initialize task ( Wait until first necessary topic is published ) ++ while(ros::ok()){ ++ if(map_loaded == 1) break; ++ ros::spinOnce(); ++ r.sleep(); ++ } ++ ++ map_sub.shutdown(); + +- ros::spinOnce(); ++ // Executing task ++ while(ros::ok()){ ++ rubis::start_task_profiling(); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::finish_job(); +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ ros::spinOnce(); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ ++ r.sleep(); + } + + return 0; +diff --git a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp +index b7c00acb..8d733aa0 100644 +--- a/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp ++++ b/autoware.ai/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp +@@ -48,8 +48,6 @@ + #include + #include + +-#include +- + #include + #include + #include +@@ -80,6 +78,7 @@ + #include + #include + #include ++#include + + #define SPIN_PROFILING + +@@ -165,18 +164,16 @@ static geometry_msgs::PoseStamped predict_pose_imu_odom_msg; + static ros::Publisher ndt_pose_pub; + static geometry_msgs::PoseStamped ndt_pose_msg; + +-static ros::Publisher rubis_ndt_pose_pub; +-static rubis_msgs::PoseStamped rubis_ndt_pose_msg; +- + // current_pose is published by vel_pose_mux + /* + static ros::Publisher current_pose_pub; + static geometry_msgs::PoseStamped current_pose_msg; + */ + +-static ros::Publisher localizer_pose_pub, rubis_localizer_pose_pub; ++static ros::Publisher rubis_pose_twist_pub; ++static ros::Publisher localizer_pose_pub; + static geometry_msgs::PoseStamped localizer_pose_msg; +-static rubis_msgs::PoseStamped rubis_localizer_pose_msg; ++static rubis_msgs::PoseTwistStamped rubis_pose_twist_msg; + + static ros::Publisher estimate_twist_pub; + static geometry_msgs::TwistStamped estimate_twist_msg; +@@ -295,7 +292,6 @@ static float _recovery_score_diff_threshold = 1.0; + static float _failure_pose_diff_threshold = 4.0; + static float _recovery_pose_diff_threshold = 1.0; + +- + void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q) + { + double cy = cos(yaw * 0.5); +@@ -1502,24 +1498,12 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) + + predict_pose_pub.publish(predict_pose_msg); + ndt_pose_pub.publish(ndt_pose_msg); +- rubis::sched::task_state_ = TASK_STATE_DONE; +- +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_ndt_pose_msg.instance = rubis::instance_; +- rubis_ndt_pose_msg.msg = ndt_pose_msg; +- rubis_ndt_pose_pub.publish(rubis_ndt_pose_msg); +- +- rubis_msgs::PoseStamped rubis_localizer_pose_msg; +- rubis_localizer_pose_msg.instance = rubis::instance_; +- rubis_localizer_pose_msg.msg = localizer_pose_msg; +- rubis_localizer_pose_pub.publish(rubis_localizer_pose_msg); +- } ++ + + // current_pose is published by vel_pose_mux + // current_pose_pub.publish(current_pose_msg); + localizer_pose_pub.publish(localizer_pose_msg); + +- + matching_end = std::chrono::system_clock::now(); + exe_time = std::chrono::duration_cast(matching_end - matching_start).count() / 1000.0; + time_ndt_matching.data = exe_time; +@@ -1556,8 +1540,10 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) + + ndt_stat_pub.publish(ndt_stat_msg); + +- +- ++ rubis_pose_twist_msg.instance = rubis::instance_; ++ rubis_pose_twist_msg.pose = ndt_pose_msg; ++ rubis_pose_twist_msg.twist = estimate_twist_msg; ++ rubis_pose_twist_pub.publish(rubis_pose_twist_msg); + + /* Compute NDT_Reliability */ + ndt_reliability.data = Wa * (exe_time / 100.0) * 100.0 + Wb * (iteration / 10.0) * 100.0 + +@@ -1714,21 +1700,21 @@ static inline void ndt_matching(const sensor_msgs::PointCloud2::ConstPtr& input) + is_kalman_filter_on_msgs.data = _is_kalman_filter_on; + is_kalman_filter_on_pub.publish(is_kalman_filter_on_msgs); + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY){ +- rubis::sched::init_task(); +- if(rubis::sched::gpu_profiling_flag_) rubis::sched::start_gpu_profiling(); +- } + } + + static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input){ +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + ndt_matching(input); + } + + static void rubis_points_callback(const rubis_msgs::PointCloud2::ConstPtr& _input){ ++ rubis::start_task_profiling(); ++ + sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared(_input->msg); + rubis::instance_ = _input->instance; + ndt_matching(input); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + static void ins_stat_callback(const rubis_msgs::InsStat::ConstPtr& input){ +@@ -1898,48 +1884,29 @@ int main(int argc, char** argv) + exit(1); + } + #endif +- +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ndt_matching.csv"); ++ + int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; ++ private_nh.param(node_name+"/rate", rate, 10); + +- int gpu_scheduling_flag; +- int gpu_profiling_flag; +- std::string gpu_execution_time_filename; +- std::string gpu_response_time_filename; +- std::string gpu_deadline_filename; ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- std::string node_name = ros::this_node::getName(); +- private_nh.param(node_name+"/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param(node_name+"/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ndt_matching.csv"); +- private_nh.param(node_name+"/rate", rate, 10); +- private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10); +- private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); +- private_nh.param(node_name+"/gpu_scheduling_flag", gpu_scheduling_flag, 0); +- private_nh.param(node_name+"/gpu_profiling_flag", gpu_profiling_flag, 0); +- private_nh.param(node_name+"/gpu_execution_time_filename", gpu_execution_time_filename, "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv"); +- private_nh.param(node_name+"/gpu_response_time_filename", gpu_response_time_filename, "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv"); +- private_nh.param(node_name+"/gpu_deadline_filename", gpu_deadline_filename, "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv"); +- private_nh.param(node_name+"/instance_mode", rubis::instance_mode_, 0); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- if(gpu_profiling_flag) rubis::sched::init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename); +- +- if( (_method_type == MethodType::PCL_ANH_GPU) && (gpu_scheduling_flag == 1) ){ +- rubis::sched::init_gpu_scheduling("/tmp/ndt_matching", gpu_deadline_filename, 0); +- } +- else if(_method_type != MethodType::PCL_ANH_GPU && gpu_scheduling_flag == 1){ +- ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!"); +- exit(1); +- } ++ rubis::init_task_profiling(task_response_time_filename); + ++ + Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z); // tl: translation + Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX()); // rot: rotation + Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY()); +@@ -1963,10 +1930,9 @@ int main(int argc, char** argv) + kalman_filtered_pose_pub = nh.advertise("/kalman_filtered_pose", 10); + + ndt_pose_pub = nh.advertise("/ndt_pose", 10); +- if(rubis::instance_mode_) rubis_ndt_pose_pub = nh.advertise("/rubis_ndt_pose",10); ++ rubis_pose_twist_pub = nh.advertise("/rubis_current_pose_twist",10); + + localizer_pose_pub = nh.advertise("/current_pose", 10); +- rubis_localizer_pose_pub = nh.advertise("/rubis_current_pose", 10); + // localizer_pose_pub = nh.advertise("/localizer_pose", 10); + estimate_twist_pub = nh.advertise("/current_velocity", 10); + // estimate_twist_pub = nh.advertise("/estimate_twist", 10); +@@ -1979,60 +1945,31 @@ int main(int argc, char** argv) + + // Subscribers + ros::Subscriber param_sub = nh.subscribe("config/ndt", 10, param_callback); +- ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback); ++ // ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback); + ros::Subscriber map_sub = nh.subscribe("points_map", 1, map_callback); + ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 10, initialpose_callback); + + ros::Subscriber points_sub; +- if(rubis::instance_mode_) points_sub = nh.subscribe("rubis_filtered_points", _queue_size, rubis_points_callback); // _queue_size = 1000 +- else points_sub = nh.subscribe("filtered_points", _queue_size, points_callback); // _queue_size = 1000 ++ points_sub = nh.subscribe("rubis_filtered_points", 1, rubis_points_callback); // _queue_size = 1000 + + // ros::Subscriber odom_sub = nh.subscribe("/vehicle/odom", _queue_size * 10, odom_callback); + // ros::Subscriber imu_sub = nh.subscribe(_imu_topic.c_str(), _queue_size * 10, imu_callback); + +- ros::Subscriber ins_stat_sub = nh.subscribe("/ins_stat", 1, ins_stat_callback); ++ // ros::Subscriber ins_stat_sub = nh.subscribe("/ins_stat", 1, ins_stat_callback); + + pthread_t thread; + pthread_create(&thread, NULL, thread_func, NULL); + +- // SPIN +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); ++ ros::Rate r(rate); ++ while(ros::ok()){ ++ if(map_loaded == 1) break; ++ ros::spinOnce(); ++ r.sleep(); + } +- else{ +- ros::Rate r(rate); +- +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(map_loaded == 1) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- map_sub.shutdown(); +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::start_job(); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); ++ ++ map_sub.shutdown(); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(gpu_profiling_flag || gpu_scheduling_flag) rubis::sched::finish_job(); +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ ros::spin(); + + return 0; + } +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/CHANGELOG.rst b/autoware.ai/src/autoware/core_perception/ndt_gpu/CHANGELOG.rst +deleted file mode 100644 +index 5c32e665..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/CHANGELOG.rst ++++ /dev/null +@@ -1,185 +0,0 @@ +-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +-Changelog for package ndt_gpu +-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +- +-1.11.0 (2019-03-21) +-------------------- +-* Fix license notice in corresponding package.xml +-* Contributors: amc-nu +- +-1.10.0 (2019-01-17) +-------------------- +- +-1.9.1 (2018-11-06) +------------------- +- +-1.9.0 (2018-10-31) +------------------- +- +-1.8.0 (2018-08-31) +------------------- +-* [Feature] Cross compile for NVIDIA DriveWorks (`#1447 `_) +-* Fix ndt not working correctly on drivePx2 and JetsonTX2 +-* Fix ndt_matching/ndt_mapping on drivePX2 and JetsonTX2 +-* Contributors: Esteve Fernandez, anhnv3991 +- +-1.7.0 (2018-05-18) +------------------- +-* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst +-* Removed unnecessary pakcage from ndt +-* Fix the bug ndt_mapping/matching use GPU resources even when selecting pcl_generic. +-* [fix] Fixes for all packages and dependencies (`#1240 `_) +- * Initial Cleanup +- * fixed also for indigo +- * kf cjeck +- * Fix road wizard +- * Added travis ci +- * Trigger CI +- * Fixes to cv_tracker and lidar_tracker cmake +- * Fix kitti player dependencies +- * Removed unnecessary dependencies +- * messages fixing for can +- * Update build script travis +- * Travis Path +- * Travis Paths fix +- * Travis test +- * Eigen checks +- * removed unnecessary dependencies +- * Eigen Detection +- * Job number reduced +- * Eigen3 more fixes +- * More Eigen3 +- * Even more Eigen +- * find package cmake modules included +- * More fixes to cmake modules +- * Removed non ros dependency +- * Enable industrial_ci for indidog and kinetic +- * Wrong install command +- * fix rviz_plugin install +- * FastVirtualScan fix +- * Fix Qt5 Fastvirtualscan +- * Fixed qt5 system dependencies for rosdep +- * NDT TKU Fix catkin not pacakged +- * More in detail dependencies fixes for more packages +- * GLEW library for ORB +- * Ignore OrbLocalizer +- * Ignore Version checker +- * Fix for driveworks interface +- * driveworks not catkinpackagedd +- * Missing catkin for driveworks +- * libdpm opencv not catkin packaged +- * catkin lib gnss not included in obj_db +- * Points2Polygon fix +- * More missing dependencies +- * image viewer not packaged +- * Fixed SSH2 detection, added viewers for all distros +- * Fix gnss localizer incorrect dependency config +- * Fixes to multiple packages dependencies +- * gnss plib and package +- * More fixes to gnss +- * gnss dependencies for gnss_loclaizer +- * Missing gnss dependency for gnss on localizer +- * More fixes for dependencies +- Replaced gnss for autoware_gnss_library +- * gnss more fixes +- * fixes to more dependencies +- * header dependency +- * Debug message +- * more debug messages changed back to gnss +- * debud messages +- * gnss test +- * gnss install command +- * Several fixes for OpenPlanner and its lbiraries +- * Fixes to ROSInterface +- * More fixes to robotsdk and rosinterface +- * robotsdk calibration fix +- * Fixes to rosinterface robotsdk libraries and its nodes +- * Fixes to Qt5 missing dependencies in robotsdk +- * glviewer missing dependencies +- * Missing qt specific config cmake for robotsdk +- * disable cv_tracker +- * Fix to open planner un needed dependendecies +- * Fixes for libraries indecision maker +- * Fixes to libraries decision_maker installation +- * Gazebo on Kinetic +- * Added Missing library +- * * Removed Gazebo and synchonization packages +- * Renames vmap in lane_planner +- * Added installation commands for missing pakcages +- * Fixes to lane_planner +- * Added NDT TKU Glut extra dependencies +- * ndt localizer/lib fast pcl fixes +- re enable cv_tracker +- * Fix kf_lib +- * Keep industrial_ci +- * Fixes for dpm library +- * Fusion lib fixed +- * dpm and fusion header should match exported project name +- * Fixes to dpm_ocv ndt_localizer and pcl_omp +- * no fast_pcl anymore +- * fixes to libdpm and its package +- * CI test +- * test with native travis ci +- * missing update for apt +- * Fixes to pcl_omp installation and headers +- * Final fixes for tests, modified README +- * * Fixes to README +- * Enable industrial_ci +- * re enable native travis tests +-* Contributors: Abraham Monrroy, Kosuke Murakami, amc-nu, anhnv3991 +- +-1.6.3 (2018-03-06) +------------------- +- +-1.6.2 (2018-02-27) +------------------- +-* Update CHANGELOG +-* Contributors: Yusuke FUJII +- +-1.6.1 (2018-01-20) +------------------- +-* update CHANGELOG +-* Contributors: Yusuke FUJII +- +-1.6.0 (2017-12-11) +------------------- +-* Prepare release for 1.6.0 +-* adapted the version to the current version +-* GPU NormalDistributionsTransform optimization +-* fix memory error +-* remove inline functions +-* add ndt_gpu in fast_pcl library +-* Contributors: Yamato ANDO, anhnv-3991, yukikitsukawa +- +-1.5.1 (2017-09-25) +------------------- +- +-1.5.0 (2017-09-21) +------------------- +- +-1.4.0 (2017-08-04) +------------------- +- +-1.3.1 (2017-07-16) +------------------- +- +-1.3.0 (2017-07-14) +------------------- +- +-1.2.0 (2017-06-07) +------------------- +- +-1.1.2 (2017-02-27 23:10) +------------------------- +- +-1.1.1 (2017-02-27 22:25) +------------------------- +- +-1.1.0 (2017-02-24) +------------------- +- +-1.0.1 (2017-01-14) +------------------- +- +-1.0.0 (2016-12-22) +------------------- +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt b/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt +deleted file mode 100644 +index 5f60e19d..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt ++++ /dev/null +@@ -1,109 +0,0 @@ +-cmake_minimum_required(VERSION 2.8.3) +-project(ndt_gpu) +- +-find_package(autoware_build_flags REQUIRED) +-find_package(PCL REQUIRED) +-find_package(CUDA) +-find_package(catkin REQUIRED COMPONENTS rubis_lib) +- +-find_package(Eigen3 QUIET) +- +-if(NOT EIGEN3_FOUND) +- # Fallback to cmake_modules +- find_package(cmake_modules REQUIRED) +- find_package(Eigen REQUIRED) +- set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +- set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only +- # Possibly map additional variables to the EIGEN3_ prefix. +-else() +- set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +-endif() +- +-AW_CHECK_CUDA() +-if(USE_CUDA) +- +- set_directory_properties(PROPERTIES COMPILE_DEFINITIONS "") +- +- if(CMAKE_CROSSCOMPILING) +- if(NOT CUDA_ARCH) +- message(FATAL_ERROR "Please define the CUDA_ARCH CMake variable") +- endif() +- else() +- if(NOT DEFINED CUDA_CAPABILITY_VERSION_CHECKER) +- set(CUDA_CAPABILITY_VERSION_CHECKER +- "${CATKIN_DEVEL_PREFIX}/lib/capability_version_checker" +- ) +- endif() +- +- execute_process(COMMAND ${CUDA_CAPABILITY_VERSION_CHECKER} +- OUTPUT_VARIABLE CUDA_CAPABILITY_VERSION +- OUTPUT_STRIP_TRAILING_WHITESPACE +- ) +- +- if("${CUDA_CAPABILITY_VERSION}" MATCHES "^[1-9][0-9]+$") +- set(CUDA_ARCH "sm_${CUDA_CAPABILITY_VERSION}") +- else() +- set(CUDA_ARCH "sm_52") +- endif() +- endif() +- +- set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-arch=${CUDA_ARCH};-std=c++11;--ptxas-options=-v) +- +- set(SUBSYS_DESC "Point cloud ndt gpu library") +- +- catkin_package( +- DEPENDS PCL #Non-catkin CMake projects +- INCLUDE_DIRS include #The exported include paths +- LIBRARIES ndt_gpu #The exported libraries from the project +- ) +- +- include_directories( +- include +- ${PCL_INCLUDE_DIRS} +- ${catkin_INCLUDE_DIRS} +- ${CUDA_INCLUDE_DIRS} +- ${EIGEN3_INCLUDE_DIRS} +- ) +- +- set(srcs +- src/MatrixDevice.cu +- src/MatrixHost.cu +- src/NormalDistributionsTransform.cu +- src/Registration.cu +- src/VoxelGrid.cu +- src/SymmetricEigenSolver.cu +- ) +- +- set(incs +- include/ndt_gpu/common.h +- include/ndt_gpu/debug.h +- include/ndt_gpu/Matrix.h +- include/ndt_gpu/MatrixDevice.h +- include/ndt_gpu/MatrixHost.h +- include/ndt_gpu/NormalDistributionsTransform.h +- include/ndt_gpu/Registration.h +- include/ndt_gpu/SymmetricEigenSolver.h +- include/ndt_gpu/VoxelGrid.h +- ) +- +- cuda_add_library(ndt_gpu ${srcs} ${incs}) +- +- target_link_libraries(ndt_gpu +- ${CUDA_LIBRARIES} +- ${CUDA_CUBLAS_LIBRARIES} +- ${CUDA_curand_LIBRARY} +- ${PCL_LIBRARIES} +- ${catkin_LIBRARIES} +- ) +- +- install(DIRECTORY include/${PROJECT_NAME}/ +- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +- FILES_MATCHING PATTERN "*.h" +- ) +- +- install(TARGETS ndt_gpu +- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +- ) +-endif() +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt.origin b/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt.origin +deleted file mode 100644 +index cc923973..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/CMakeLists.txt.origin ++++ /dev/null +@@ -1,110 +0,0 @@ +-cmake_minimum_required(VERSION 2.8.3) +-project(ndt_gpu) +- +-find_package(autoware_build_flags REQUIRED) +-find_package(catkin REQUIRED) +-find_package(PCL REQUIRED) +-find_package(CUDA) +- +-find_package(Eigen3 QUIET) +- +-if(NOT EIGEN3_FOUND) +- # Fallback to cmake_modules +- find_package(cmake_modules REQUIRED) +- find_package(Eigen REQUIRED) +- set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +- set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only +- # Possibly map additional variables to the EIGEN3_ prefix. +-else() +- set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) +-endif() +- +-AW_CHECK_CUDA() +- +-if(USE_CUDA) +- set_directory_properties(PROPERTIES COMPILE_DEFINITIONS "") +- +- if(CMAKE_CROSSCOMPILING) +- if(NOT CUDA_ARCH) +- message(FATAL_ERROR "Please define the CUDA_ARCH CMake variable") +- endif() +- else() +- if(NOT DEFINED CUDA_CAPABILITY_VERSION_CHECKER) +- set(CUDA_CAPABILITY_VERSION_CHECKER +- "${CATKIN_DEVEL_PREFIX}/lib/capability_version_checker" +- ) +- endif() +- +- execute_process(COMMAND ${CUDA_CAPABILITY_VERSION_CHECKER} +- OUTPUT_VARIABLE CUDA_CAPABILITY_VERSION +- OUTPUT_STRIP_TRAILING_WHITESPACE +- ) +- +- if("${CUDA_CAPABILITY_VERSION}" MATCHES "^[1-9][0-9]+$") +- set(CUDA_ARCH "sm_${CUDA_CAPABILITY_VERSION}") +- else() +- set(CUDA_ARCH "sm_52") +- endif() +- endif() +- +- set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS};-arch=${CUDA_ARCH};-std=c++11;--ptxas-options=-v) +- +- set(SUBSYS_DESC "Point cloud ndt gpu library") +- +- catkin_package( +- DEPENDS PCL #Non-catkin CMake projects +- INCLUDE_DIRS include #The exported include paths +- LIBRARIES ndt_gpu #The exported libraries from the project +- ) +- +- include_directories( +- include +- ${PCL_INCLUDE_DIRS} +- ${catkin_INCLUDE_DIRS} +- ${CUDA_INCLUDE_DIRS} +- ${EIGEN3_INCLUDE_DIRS} +- ) +- +- set(srcs +- src/MatrixDevice.cu +- src/MatrixHost.cu +- src/NormalDistributionsTransform.cu +- src/Registration.cu +- src/VoxelGrid.cu +- src/SymmetricEigenSolver.cu +- ) +- +- set(incs +- include/ndt_gpu/common.h +- include/ndt_gpu/debug.h +- include/ndt_gpu/Matrix.h +- include/ndt_gpu/MatrixDevice.h +- include/ndt_gpu/MatrixHost.h +- include/ndt_gpu/NormalDistributionsTransform.h +- include/ndt_gpu/Registration.h +- include/ndt_gpu/SymmetricEigenSolver.h +- include/ndt_gpu/VoxelGrid.h +- ) +- +- cuda_add_library(ndt_gpu ${srcs} ${incs}) +- +- target_link_libraries(ndt_gpu +- ${CUDA_LIBRARIES} +- ${CUDA_CUBLAS_LIBRARIES} +- ${CUDA_curand_LIBRARY} +- ${PCL_LIBRARIES} +- ) +- +- install(DIRECTORY include/${PROJECT_NAME}/ +- DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +- FILES_MATCHING PATTERN "*.h" +- ) +- +- install(TARGETS ndt_gpu +- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +- ) +-else() +- message("ndt_gpu will not be built, CUDA was not found.") +-endif() +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Matrix.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Matrix.h +deleted file mode 100644 +index 16e5033d..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Matrix.h ++++ /dev/null +@@ -1,255 +0,0 @@ +-#ifndef GMATRIX_H_ +-#define GMATRIX_H_ +- +-#include +-#include +-#include "common.h" +-#include +- +-namespace gpu { +- +-class Matrix { +-public: +- CUDAH Matrix(); +- +- CUDAH Matrix(int rows, int cols, int offset, double *buffer); +- +- CUDAH int rows() const; +- +- CUDAH int cols() const; +- +- CUDAH int offset() const; +- +- CUDAH double *buffer() const; +- +- CUDAH void setRows(int rows); +- CUDAH void setCols(int cols); +- CUDAH void setOffset(int offset); +- CUDAH void setBuffer(double *buffer); +- CUDAH void setCellVal(int row, int col, double val); +- +- CUDAH void copy(Matrix &output); +- +- //Need to fix. Only reducing rows is OK now. +- CUDAH void resize(int rows, int cols); +- +- CUDAH double *cellAddr(int row, int col); +- +- CUDAH double *cellAddr(int index); +- +- //Assignment operator +- CUDAH void operator=(const Matrix input); +- +- CUDAH double& operator()(int row, int col); +- +- CUDAH void set(int row, int col, double val); +- +- CUDAH double& operator()(int index); +- +- CUDAH double at(int row, int col) const; +- +- CUDAH bool operator*=(double val); +- +- CUDAH bool operator/=(double val); +- +- CUDAH bool transpose(Matrix &output); +- +- //Only applicable for 3x3 matrix or below +- CUDAH bool inverse(Matrix &output); +- +- CUDAH Matrix col(int index); +- +- CUDAH Matrix row(int index); +- +-protected: +- double *buffer_; +- int rows_, cols_, offset_; +-}; +- +- +-CUDAH Matrix::Matrix() { +- buffer_ = NULL; +- rows_ = cols_ = offset_ = 0; +-} +- +-CUDAH Matrix::Matrix(int rows, int cols, int offset, double *buffer) { +- rows_ = rows; +- cols_ = cols; +- offset_ = offset; +- buffer_ = buffer; +-} +- +-CUDAH int Matrix::rows() const { +- return rows_; +-} +- +-CUDAH int Matrix::cols() const { +- return cols_; +-} +- +-CUDAH int Matrix::offset() const { +- return offset_; +-} +- +-CUDAH double *Matrix::buffer() const { +- return buffer_; +-} +- +-CUDAH void Matrix::setRows(int rows) { rows_ = rows; } +-CUDAH void Matrix::setCols(int cols) { cols_ = cols; } +-CUDAH void Matrix::setOffset(int offset) { offset_ = offset; } +-CUDAH void Matrix::setBuffer(double *buffer) { buffer_ = buffer; } +-CUDAH void Matrix::setCellVal(int row, int col, double val) { +- buffer_[(row * cols_ + col) * offset_] = val; +-} +- +-CUDAH void Matrix::copy(Matrix &output) { +- for (int i = 0; i < rows_; i++) { +- for (int j = 0; j < cols_; j++) { +- output(i, j) = buffer_[(i * cols_ + j) * offset_]; +- } +- } +-} +- +-//Need to fix. Only reducing rows is OK now. +-CUDAH void Matrix::resize(int rows, int cols) { +- rows_ = rows; +- cols_ = cols; +-} +- +-CUDAH double *Matrix::cellAddr(int row, int col) { +- if (row >= rows_ || col >= cols_ || row < 0 || col < 0) +- return NULL; +- +- return buffer_ + (row * cols_ + col) * offset_; +-} +- +-CUDAH double *Matrix::cellAddr(int index) { +- if (rows_ == 1 && index >= 0 && index < cols_) { +- return buffer_ + index * offset_; +- } +- else if (cols_ == 1 && index >= 0 && index < rows_) { +- return buffer_ + index * offset_; +- } +- +- return NULL; +-} +- +-//Assignment operator +-CUDAH void Matrix::operator=(const Matrix input) { +- rows_ = input.rows_; +- cols_ = input.cols_; +- offset_ = input.offset_; +- buffer_ = input.buffer_; +-} +- +-CUDAH double& Matrix::operator()(int row, int col) { +- return buffer_[(row * cols_ + col) * offset_]; +-} +- +-CUDAH void Matrix::set(int row, int col, double val) { +- buffer_[(row * cols_ + col) * offset_] = val; +-} +- +-CUDAH double& Matrix::operator()(int index) { +- return buffer_[index * offset_]; +-} +- +-CUDAH double Matrix::at(int row, int col) const { +- return buffer_[(row * cols_ + col) * offset_]; +-} +- +-CUDAH bool Matrix::operator*=(double val) { +- for (int i = 0; i < rows_; i++) { +- for (int j = 0; j < cols_; j++) { +- buffer_[(i * cols_ + j) * offset_] *= val; +- } +- } +- +- return true; +-} +- +-CUDAH bool Matrix::operator/=(double val) { +- if (val == 0) +- return false; +- +- for (int i = 0; i < rows_ * cols_; i++) { +- buffer_[i * offset_] /= val; +- } +- +- return true; +-} +- +-CUDAH bool Matrix::transpose(Matrix &output) { +- if (rows_ != output.cols_ || cols_ != output.rows_) +- return false; +- +- for (int i = 0; i < rows_; i++) { +- for (int j = 0; j < cols_; j++) { +- output(j, i) = buffer_[(i * cols_ + j) * offset_]; +- } +- } +- +- return true; +-} +- +-//Only applicable for 3x3 matrix or below +-CUDAH bool Matrix::inverse(Matrix &output) { +- if (rows_ != cols_ || rows_ == 0 || cols_ == 0) +- return false; +- +- if (rows_ == 1) { +- if (buffer_[0] != 0) +- output(0, 0) = 1 / buffer_[0]; +- else +- return false; +- } +- +- if (rows_ == 2) { +- double det = at(0, 0) * at(1, 1) - at(0, 1) * at(1, 0); +- +- if (det != 0) { +- output(0, 0) = at(1, 1) / det; +- output(0, 1) = - at(0, 1) / det; +- +- output(1, 0) = - at(1, 0) / det; +- output(1, 1) = at(0, 0) / det; +- } else +- return false; +- } +- +- if (rows_ == 3) { +- double det = at(0, 0) * at(1, 1) * at(2, 2) + at(0, 1) * at(1, 2) * at(2, 0) + at(1, 0) * at (2, 1) * at(0, 2) +- - at(0, 2) * at(1, 1) * at(2, 0) - at(0, 1) * at(1, 0) * at(2, 2) - at(0, 0) * at(1, 2) * at(2, 1); +- double idet = 1.0 / det; +- +- if (det != 0) { +- output(0, 0) = (at(1, 1) * at(2, 2) - at(1, 2) * at(2, 1)) * idet; +- output(0, 1) = - (at(0, 1) * at(2, 2) - at(0, 2) * at(2, 1)) * idet; +- output(0, 2) = (at(0, 1) * at(1, 2) - at(0, 2) * at(1, 1)) * idet; +- +- output(1, 0) = - (at(1, 0) * at(2, 2) - at(1, 2) * at(2, 0)) * idet; +- output(1, 1) = (at(0, 0) * at(2, 2) - at(0, 2) * at(2, 0)) * idet; +- output(1, 2) = - (at(0, 0) * at(1, 2) - at(0, 2) * at(1, 0)) * idet; +- +- output(2, 0) = (at(1, 0) * at(2, 1) - at(1, 1) * at(2, 0)) * idet; +- output(2, 1) = - (at(0, 0) * at(2, 1) - at(0, 1) * at(2, 0)) * idet; +- output(2, 2) = (at(0, 0) * at(1, 1) - at(0, 1) * at(1, 0)) * idet; +- } else +- return false; +- } +- +- return true; +-} +- +-CUDAH Matrix Matrix::col(int index) { +- return Matrix(rows_, 1, offset_ * cols_, buffer_ + index * offset_); +-} +- +-CUDAH Matrix Matrix::row(int index) { +- return Matrix(1, cols_, offset_, buffer_ + index * cols_ * offset_); +-} +- +-} +- +-#endif +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixDevice.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixDevice.h +deleted file mode 100644 +index f48cbf60..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixDevice.h ++++ /dev/null +@@ -1,83 +0,0 @@ +-#ifndef MATRIX_DEVICE_H_ +-#define MATRIX_DEVICE_H_ +- +-#include "Matrix.h" +- +-namespace gpu { +-class MatrixDevice : public Matrix { +-public: +- int memAllocId; +- int memFreeId; +- +- CUDAH MatrixDevice(); +- +- MatrixDevice(int rows, int cols); +- +- CUDAH MatrixDevice(int rows, int cols, int offset, double *buffer); +- +- CUDAH bool isEmpty(); +- +- CUDAH MatrixDevice col(int index); +- +- CUDAH MatrixDevice row(int index); +- +- CUDAH void setBuffer(double *buffer); +- +- void memAlloc(); +- +- void memAlloc_free(); +- void memAlloc_malloc(); +- void memAlloc_memset(); +- +- void memFree(); +- +-private: +- bool fr_; +-}; +- +-CUDAH MatrixDevice::MatrixDevice() +-{ +- rows_ = cols_ = offset_ = 0; +- buffer_ = NULL; +- fr_ = true; +-} +- +-CUDAH MatrixDevice::MatrixDevice(int rows, int cols, int offset, double *buffer) +-{ +- rows_ = rows; +- cols_ = cols; +- offset_ = offset; +- buffer_ = buffer; +- fr_ = false; +-} +- +-CUDAH bool MatrixDevice::isEmpty() +-{ +- return (rows_ == 0 || cols_ == 0 || buffer_ == NULL); +-} +- +-CUDAH MatrixDevice MatrixDevice::col(int index) +-{ +- return MatrixDevice(rows_, 1, offset_ * cols_, buffer_ + index * offset_); +-} +- +-CUDAH MatrixDevice MatrixDevice::row(int index) +-{ +- return MatrixDevice(1, cols_, offset_, buffer_ + index * cols_ * offset_); +-} +- +-CUDAH void MatrixDevice::setBuffer(double *buffer) +-{ +- buffer_ = buffer; +-} +- +- +- +-class SquareMatrixDevice : public MatrixDevice { +-public: +- SquareMatrixDevice(int size); +-}; +- +-} +- +-#endif +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixHost.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixHost.h +deleted file mode 100644 +index fb359a31..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/MatrixHost.h ++++ /dev/null +@@ -1,37 +0,0 @@ +-#ifndef MATRIX_HOST_H_ +-#define MATRIX_HOST_H_ +- +-#include "Matrix.h" +-#include "MatrixDevice.h" +- +-namespace gpu { +-class MatrixHost : public Matrix { +-public: +- int moveToGpuId; +- int moveToHostId; +- +- MatrixHost(); +- MatrixHost(int rows, int cols); +- MatrixHost(int rows, int cols, int offset, double *buffer); +- MatrixHost(const MatrixHost& other); +- +- bool moveToGpu(MatrixDevice output); +- bool moveToHost(MatrixDevice input); +- +- MatrixHost &operator=(const MatrixHost &other); +- +- void debug(); +- +- ~MatrixHost(); +-private: +- bool fr_; +-}; +- +-class SquareMatrixHost: public MatrixHost { +-public: +- SquareMatrixHost(int size); +-}; +- +-} +- +-#endif +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/NormalDistributionsTransform.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/NormalDistributionsTransform.h +deleted file mode 100644 +index 6b7ecf97..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/NormalDistributionsTransform.h ++++ /dev/null +@@ -1,112 +0,0 @@ +-#ifndef GPU_NDT_H_ +-#define GPU_NDT_H_ +- +-#include +-#include +-#include "Registration.h" +-#include "common.h" +-#include "VoxelGrid.h" +-#include "Eigen/Geometry" +-#include "rubis_lib/sched.hpp" +- +-using namespace rubis; +- +-namespace gpu { +- +-class GNormalDistributionsTransform: public GRegistration { +-public: +- GNormalDistributionsTransform(); +- +- GNormalDistributionsTransform(const GNormalDistributionsTransform &other); +- +- void setStepSize(double step_size); +- +- void setResolution(float resolution); +- +- void setOutlierRatio(double olr); +- +- double getStepSize() const; +- +- float getResolution() const; +- +- double getOutlierRatio() const; +- +- double getTransformationProbability() const; +- +- int getRealIterations(); +- +- /* Set the input map points */ +- void setInputTarget(pcl::PointCloud::Ptr input); +- void setInputTarget(pcl::PointCloud::Ptr input); +- +- /* Compute and get fitness score */ +- double getFitnessScore(double max_range = DBL_MAX); +- +- ~GNormalDistributionsTransform(); +- +- +-protected: +- void computeTransformation(const Eigen::Matrix &guess); +- double computeDerivatives(Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, +- float *trans_x, float *trans_y, float *trans_z, +- int points_num, Eigen::Matrix pose, bool compute_hessian = true); +- +-private: +- //Copied from ndt.h +- double auxilaryFunction_PsiMT (double a, double f_a, double f_0, double g_0, double mu = 1.e-4); +- +- //Copied from ndt.h +- double auxilaryFunction_dPsiMT (double g_a, double g_0, double mu = 1.e-4); +- +- double updateIntervalMT (double &a_l, double &f_l, double &g_l, +- double &a_u, double &f_u, double &g_u, +- double a_t, double f_t, double g_t); +- +- double trialValueSelectionMT (double a_l, double f_l, double g_l, +- double a_u, double f_u, double g_u, +- double a_t, double f_t, double g_t); +- +- void transformPointCloud(float *in_x, float *in_y, float *in_z, +- float *out_x, float *out_y, float *out_z, +- int points_number, Eigen::Matrix transform); +- +- void computeAngleDerivatives(MatrixHost pose, bool compute_hessian = true); +- +- double computeStepLengthMT(const Eigen::Matrix &x, Eigen::Matrix &step_dir, +- double step_init, double step_max, double step_min, double &score, +- Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, +- float *out_x, float *out_y, float *out_z, int points_num); +- +- void computeHessian(Eigen::Matrix &hessian, float *trans_x, float *trans_y, float *trans_z, int points_num, Eigen::Matrix &p); +- +- +- double gauss_d1_, gauss_d2_; +- double outlier_ratio_; +- //MatrixHost j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_; +- MatrixHost j_ang_; +- +- //MatrixHost h_ang_a2_, h_ang_a3_, h_ang_b2_, h_ang_b3_, h_ang_c2_, h_ang_c3_, h_ang_d1_, h_ang_d2_, h_ang_d3_, +- // h_ang_e1_, h_ang_e2_, h_ang_e3_, h_ang_f1_, h_ang_f2_, h_ang_f3_; +- MatrixHost h_ang_; +- +- +- //MatrixDevice dj_ang_a_, dj_ang_b_, dj_ang_c_, dj_ang_d_, dj_ang_e_, dj_ang_f_, dj_ang_g_, dj_ang_h_; +- MatrixDevice dj_ang_; +- +- +- //MatrixDevice dh_ang_a2_, dh_ang_a3_, dh_ang_b2_, dh_ang_b3_, dh_ang_c2_, dh_ang_c3_, dh_ang_d1_, dh_ang_d2_, dh_ang_d3_, +- // dh_ang_e1_, dh_ang_e2_, dh_ang_e3_, dh_ang_f1_, dh_ang_f2_, dh_ang_f3_; +- MatrixDevice dh_ang_; +- +- double step_size_; +- float resolution_; +- double trans_probability_; +- +- int real_iterations_; +- +- +- GVoxelGrid voxel_grid_; +-}; +-} +- +-#endif +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Registration.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Registration.h +deleted file mode 100644 +index 4eea6185..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/Registration.h ++++ /dev/null +@@ -1,98 +0,0 @@ +-#ifndef GNDT_H_ +-#define GNDT_H_ +- +-#include +-#include +-#include "Matrix.h" +-#include "MatrixHost.h" +-#include "MatrixDevice.h" +-#include "common.h" +-#include +-#include +-#include +-#include +-#include +-#include +- +-#define HTOD 0 +-#define DTOH 1 +-#define LAUNCH 2 +-#define GPU_PROFILING 1 +- +-using namespace rubis; +- +-void start_profiling_execution_time(); +-void start_profiling_response_time(); +-void start_profiling_cpu_time(); +-void stop_cpu_profiling(); +-void stop_profiling(int id, int type); +-void write_profiling_data(const char* id, float e_time, float r_time, int type); +-void write_cpu_profiling_data(const char *id, long long int c_time); +-void write_dummy_line(); +-void initialize_file(const char* execution_time_filename, const char* response_time_filename, const char* remain_time_filename); +-void close_file(); +- +-namespace gpu { +-class GRegistration { +-public: +- GRegistration(); +- GRegistration(const GRegistration &other); +- +- void align(const Eigen::Matrix &guess); +- +- void setTransformationEpsilon(double trans_eps); +- +- double getTransformationEpsilon() const; +- +- void setMaximumIterations(int max_itr); +- +- int getMaximumIterations() const; +- +- Eigen::Matrix getFinalTransformation() const; +- +- /* Set input Scanned point cloud. +- * Copy input points from the main memory to the GPU memory */ +- void setInputSource(pcl::PointCloud::Ptr input); +- void setInputSource(pcl::PointCloud::Ptr input); +- +- /* Set input reference map point cloud. +- * Copy input points from the main memory to the GPU memory */ +- void setInputTarget(pcl::PointCloud::Ptr input); +- void setInputTarget(pcl::PointCloud::Ptr input); +- +- int getFinalNumIteration() const; +- +- bool hasConverged() const; +- +- virtual ~GRegistration(); +- +-protected: +- +- virtual void computeTransformation(const Eigen::Matrix &guess); +- +- double transformation_epsilon_; +- int max_iterations_; +- +- //Original scanned point clouds +- float *x_, *y_, *z_; +- int points_number_; +- +- //Transformed point clouds +- float *trans_x_, *trans_y_, *trans_z_; +- +- bool converged_; +- int nr_iterations_; +- +- Eigen::Matrix final_transformation_, transformation_, previous_transformation_; +- +- bool target_cloud_updated_; +- +- // Reference map point +- float *target_x_, *target_y_, *target_z_; +- int target_points_number_; +- +- bool is_copied_; +-}; +-} +- +-#endif +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/SymmetricEigenSolver.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/SymmetricEigenSolver.h +deleted file mode 100644 +index 2cf5faa3..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/SymmetricEigenSolver.h ++++ /dev/null +@@ -1,416 +0,0 @@ +-#ifndef GSYMMETRIC_EIGEN_ +-#define GSYMMETRIC_EIGEN_ +- +-#include +-#include +-#include "MatrixDevice.h" +-#include +- +-namespace gpu { +- +-class SymmetricEigensolver3x3 { +-public: +- CUDAH SymmetricEigensolver3x3(); +- +- SymmetricEigensolver3x3(int offset); +- +- CUDAH SymmetricEigensolver3x3(const SymmetricEigensolver3x3& other); +- +- void setInputMatrices(double *input_matrices); +- +- void setEigenvectors(double *eigenvectors); +- +- void setEigenvalues(double *eigenvalues); +- +- double *getBuffer() const; +- +- /* Normalize input matrices by dividing each matrix +- * to the element that has the largest absolute value +- * in each matrix */ +- CUDAH void normalizeInput(int tid); +- +- /* Compute eigenvalues */ +- CUDAH void computeEigenvalues(int tid); +- +- /* First step to compute the eigenvector 0 +- * Because computing the eigenvector 0 using +- * only one kernel is too expensive (which causes +- * the "too many resources requested for launch" error, +- * I have to divide them into two distinct kernels. */ +- CUDAH void computeEigenvector00(int tid); +- +- /* Second step to compute the eigenvector 0 */ +- CUDAH void computeEigenvector01(int tid); +- +- /* First step to compute the eigenvector 1 */ +- CUDAH void computeEigenvector10(int tid); +- +- /* Second step to compute the eigenvector 1 */ +- CUDAH void computeEigenvector11(int tid); +- +- /* Compute the final eigenvector by crossing +- * eigenvector 0 and eigenvector 1 */ +- CUDAH void computeEigenvector2(int tid); +- +- /* Final step to compute eigenvalues */ +- CUDAH void updateEigenvalues(int tid); +- +- /* Free memory */ +- void memFree(); +- +-private: +- CUDAH void computeOrthogonalComplement(MatrixDevice w, MatrixDevice u, MatrixDevice v); +- +- //Operators +- CUDAH void multiply(MatrixDevice u, double mult, MatrixDevice output); +- +- CUDAH void subtract(MatrixDevice u, MatrixDevice v, MatrixDevice output); +- +- CUDAH void divide(MatrixDevice u, double div, MatrixDevice output); +- +- CUDAH double dot(MatrixDevice u, MatrixDevice v); +- +- CUDAH void cross(MatrixDevice in0, MatrixDevice in1, MatrixDevice out); +- +- int offset_; +- +- // Buffers for intermediate calculation +- double *buffer_; +- int *i02_; +- double *maxAbsElement_; +- double *norm_; +- +- double *eigenvectors_; +- double *eigenvalues_; +- double *input_matrices_; +- +- bool is_copied_; +-}; +- +- +-CUDAH SymmetricEigensolver3x3::SymmetricEigensolver3x3() +-{ +- buffer_ = NULL; +- eigenvectors_ = NULL; +- eigenvalues_ = NULL; +- input_matrices_ = NULL; +- maxAbsElement_ = NULL; +- norm_ = NULL; +- i02_ = NULL; +- offset_ = 0; +- is_copied_ = false; +-} +- +-CUDAH SymmetricEigensolver3x3::SymmetricEigensolver3x3(const SymmetricEigensolver3x3& other) +-{ +- buffer_ = other.buffer_; +- offset_ = other.offset_; +- eigenvectors_ = other.eigenvectors_; +- eigenvalues_ = other.eigenvalues_; +- input_matrices_ = other.input_matrices_; +- +- maxAbsElement_ = other.maxAbsElement_; +- norm_ = other.norm_; +- i02_ = other.i02_; +- is_copied_ = true; +-} +- +-CUDAH void SymmetricEigensolver3x3::normalizeInput(int tid) +-{ +- MatrixDevice input(3, 3, offset_, input_matrices_ + tid); +- +- double a00 = input(0, 0); +- double a01 = input(0, 1); +- double a02 = input(0, 2); +- double a11 = input(1, 1); +- double a12 = input(1, 2); +- double a22 = input(2, 2); +- +- double max0 = (fabs(a00) > fabs(a01)) ? fabs(a00) : fabs(a01); +- double max1 = (fabs(a02) > fabs(a11)) ? fabs(a02) : fabs(a11); +- double max2 = (fabs(a12) > fabs(a22)) ? fabs(a12) : fabs(a22); +- +- double maxAbsElement = (max0 > max1) ? max0 : max1; +- +- maxAbsElement = (maxAbsElement > max2) ? maxAbsElement : max2; +- +- if (maxAbsElement == 0.0) { +- MatrixDevice evec(3, 3, offset_, eigenvectors_ + tid); +- +- evec(0, 0) = 1.0; +- evec(1, 1) = 1.0; +- evec(2, 2) = 1.0; +- +- norm_[tid] = 0.0; +- return; +- } +- +- double invMaxAbsElement = 1.0 / maxAbsElement; +- +- a00 *= invMaxAbsElement; +- a01 *= invMaxAbsElement; +- a02 *= invMaxAbsElement; +- a11 *= invMaxAbsElement; +- a12 *= invMaxAbsElement; +- a22 *= invMaxAbsElement; +- +- input(0, 0) = a00; +- input(0, 1) = a01; +- input(0, 2) = a02; +- input(1, 1) = a11; +- input(1, 2) = a12; +- input(2, 2) = a22; +- input(1, 0) = a01; +- input(2, 0) = a02; +- input(2, 1) = a12; +- +- norm_[tid] = a01 * a01 + a02 * a02 + a12 * a12; +- +- maxAbsElement_[tid] = maxAbsElement; +-} +- +-CUDAH void SymmetricEigensolver3x3::computeEigenvalues(int tid) +-{ +- MatrixDevice input(3, 3, offset_, input_matrices_ + tid); +- MatrixDevice eval(3, 1, offset_, eigenvalues_ + tid); +- +- double a00 = input(0, 0); +- double a01 = input(0, 1); +- double a02 = input(0, 2); +- double a11 = input(1, 1); +- double a12 = input(1, 2); +- double a22 = input(2, 2); +- +- double norm = norm_[tid]; +- +- if (norm > 0.0) { +- double traceDiv3 = (a00 + a11 + a22) / 3.0; +- double b00 = a00 - traceDiv3; +- double b11 = a11 - traceDiv3; +- double b22 = a22 - traceDiv3; +- double denom = sqrt((b00 * b00 + b11 * b11 + b22 * b22 + norm * 2.0) / 6.0); +- double c00 = b11 * b22 - a12 * a12; +- double c01 = a01 * b22 - a12 * a02; +- double c02 = a01 * a12 - b11 * a02; +- double det = (b00 * c00 - a01 * c01 + a02 * c02) / (denom * denom * denom); +- double halfDet = det * 0.5; +- +- halfDet = (halfDet > -1.0) ? halfDet : -1.0; +- halfDet = (halfDet < 1.0) ? halfDet : 1.0; +- +- double angle = acos(halfDet) / 3.0; +- double beta2 = cos(angle) * 2.0; +- double beta0 = cos(angle + M_PI * 2.0 / 3.0) * 2.0; +- double beta1 = -(beta0 + beta2); +- +- eval(0) = traceDiv3 + denom * beta0; +- eval(1) = traceDiv3 + denom * beta1; +- eval(2) = traceDiv3 + denom * beta2; +- +- i02_[tid] = (halfDet >= 0) ? 2 : 0; +- i02_[tid + offset_] = (halfDet >= 0) ? 0 : 2; +- +- } else { +- eval(0) = a00; +- eval(1) = a11; +- eval(2) = a22; +- } +-} +- +-CUDAH void SymmetricEigensolver3x3::updateEigenvalues(int tid) +-{ +- double maxAbsElement = maxAbsElement_[tid]; +- MatrixDevice eval(3, 1, offset_, eigenvalues_ + tid); +- +- eval(0) *= maxAbsElement; +- eval(1) *= maxAbsElement; +- eval(2) *= maxAbsElement; +-} +- +-CUDAH void SymmetricEigensolver3x3::computeEigenvector00(int tid) +-{ +- if (norm_[tid] > 0.0) { +- MatrixDevice input(3, 3, offset_, input_matrices_ + tid); +- MatrixDevice row_mat(3, 3, offset_, buffer_ + tid); +- double eval0 = eigenvalues_[tid + i02_[tid] * offset_]; +- +- input.copy(row_mat); +- +- row_mat(0, 0) -= eval0; +- row_mat(1, 1) -= eval0; +- row_mat(2, 2) -= eval0; +- +- //row0 is r0xr1, row1 is r0xr2, row2 is r1xr2 +- MatrixDevice rxr(3, 3, offset_, buffer_ + 3 * 3 * offset_ + tid); +- +- cross(row_mat.row(0), row_mat.row(1), rxr.row(0)); +- cross(row_mat.row(0), row_mat.row(2), rxr.row(1)); +- cross(row_mat.row(1), row_mat.row(2), rxr.row(2)); +- +- } else { +- eigenvectors_[tid] = 1.0; +- } +-} +- +-CUDAH void SymmetricEigensolver3x3::computeEigenvector01(int tid) +-{ +- if (norm_[tid] > 0.0) { +- MatrixDevice evec0(3, 1, offset_ * 3, eigenvectors_ + tid + i02_[tid] * offset_); +- +- //row0 is r0xr1, row1 is r0xr2, row2 is r1xr2 +- MatrixDevice rxr(3, 3, offset_, buffer_ + 3 * 3 * offset_ + tid); +- +- +- double d0 = rxr(0, 0) * rxr(0, 0) + rxr(0, 1) * rxr(0, 1) * rxr(0, 2) * rxr(0, 2); +- double d1 = rxr(1, 0) * rxr(1, 0) + rxr(1, 1) * rxr(1, 1) * rxr(1, 2) * rxr(1, 2); +- double d2 = rxr(2, 0) * rxr(2, 0) + rxr(2, 1) * rxr(2, 1) * rxr(2, 2) * rxr(2, 2); +- +- double dmax = (d0 > d1) ? d0 : d1; +- int imax = (d0 > d1) ? 0 : 1; +- +- dmax = (d2 > dmax) ? d2 : dmax; +- imax = (d2 > dmax) ? 2 : imax; +- +- divide(rxr.row(imax), sqrt(dmax), evec0); +- } +-} +- +-CUDAH void SymmetricEigensolver3x3::computeEigenvector10(int tid) +-{ +- if (norm_[tid] > 0.0) { +- MatrixDevice input(3, 3, offset_, input_matrices_ + tid); +- MatrixDevice evec0(3, 1, offset_ * 3, eigenvectors_ + tid + i02_[tid] * offset_); +- double eval1 = eigenvalues_[tid + offset_]; +- +- MatrixDevice u(3, 1, offset_, buffer_ + tid); +- MatrixDevice v(3, 1, offset_, buffer_ + 3 * offset_ + tid); +- +- computeOrthogonalComplement(evec0, u, v); +- +- MatrixDevice au(3, 1, offset_, buffer_ + 6 * offset_ + tid); +- MatrixDevice av(3, 1, offset_, buffer_ + 9 * offset_ + tid); +- +- double t0, t1, t2; +- +- t0 = u(0); +- t1 = u(1); +- t2 = u(2); +- +- au(0) = (input(0, 0) - eval1) * t0 + input(0, 1) * t1 + input(0, 2) * t2; +- au(1) = input(0, 1) * t0 + (input(1, 1) - eval1) * t1 + input(1, 2) * t2; +- au(2) = input(0, 2) * t0 + input(1, 2) * t1 + (input(2, 2) - eval1) * t2; +- +- t0 = v(0); +- t1 = v(1); +- t2 = v(2); +- +- av(0) = (input(0, 0) - eval1) * t0 + input(0, 1) * t1 + input(0, 2) * t2; +- av(1) = input(0, 1) * t0 + (input(1, 1) - eval1) * t1 + input(1, 2) * t2; +- av(2) = input(0, 2) * t0 + input(1, 2) * t1 + (input(2, 2) - eval1) * t2; +- } else { +- eigenvectors_[tid + offset_ * 4] = 1.0; +- } +-} +- +-CUDAH void SymmetricEigensolver3x3::computeEigenvector11(int tid) +-{ +- if (norm_[tid] > 0.0) { +- MatrixDevice evec1(3, 1, offset_ * 3, eigenvectors_ + tid + offset_); +- +- MatrixDevice u(3, 1, offset_, buffer_ + tid); +- MatrixDevice v(3, 1, offset_, buffer_ + 3 * offset_ + tid); +- +- MatrixDevice au(3, 1, offset_, buffer_ + 6 * offset_ + tid); +- MatrixDevice av(3, 1, offset_, buffer_ + 9 * offset_ + tid); +- +- double m00 = u(0) * au(0) + u(1) * au(1) + u(2) * au(2); +- double m01 = u(0) * av(0) + u(1) * av(1) + u(2) * av(2); +- double m11 = v(0) * av(0) + v(1) * av(1) + v(2) * av(2); +- +- double abs_m00 = fabs(m00); +- double abs_m01 = fabs(m01); +- double abs_m11 = fabs(m11); +- +- if (abs_m00 > 0 || abs_m01 > 0 || abs_m11 > 0) { +- double u_mult = (abs_m00 >= abs_m11) ? m01 : m11; +- double v_mult = (abs_m00 >= abs_m11) ? m00 : m01; +- +- bool res = fabs(u_mult) >= fabs(v_mult); +- double *large = (res) ? &u_mult : &v_mult; +- double *small = (res) ? &v_mult : &u_mult; +- +- *small /= (*large); +- *large = 1.0 / sqrt(1.0 + (*small) * (*small)); +- *small *= (*large); +- +- multiply(u, u_mult, u); +- multiply(v, v_mult, v); +- subtract(u, v, evec1); +- +- } else { +- u.copy(evec1); +- } +- } +-} +- +-CUDAH void SymmetricEigensolver3x3::multiply(MatrixDevice u, double mult, MatrixDevice output) +-{ +- output(0) = u(0) * mult; +- output(1) = u(1) * mult; +- output(2) = u(2) * mult; +-} +- +-CUDAH void SymmetricEigensolver3x3::subtract(MatrixDevice u, MatrixDevice v, MatrixDevice output) +-{ +- output(0) = u(0) - v(0); +- output(1) = u(1) - v(1); +- output(2) = u(2) - v(2); +-} +- +-CUDAH void SymmetricEigensolver3x3::divide(MatrixDevice u, double div, MatrixDevice output) +-{ +- output(0) = u(0) / div; +- output(1) = u(1) / div; +- output(2) = u(2) / div; +-} +- +-CUDAH double SymmetricEigensolver3x3::dot(MatrixDevice u, MatrixDevice v) +-{ +- return (u(0) * v(0) + u(1) * v(1) + u(2) * v(2)); +-} +- +-CUDAH void SymmetricEigensolver3x3::cross(MatrixDevice u, MatrixDevice v, MatrixDevice out) +-{ +- out(0) = u(1) * v(2) - u(2) * v(1); +- out(1) = u(2) * v(0) - u(0) * v(2); +- out(2) = u(0) * v(1) - u(1) * v(0); +-} +- +-CUDAH void SymmetricEigensolver3x3::computeOrthogonalComplement(MatrixDevice w, MatrixDevice u, MatrixDevice v) +-{ +- bool c = (fabs(w(0)) > fabs(w(1))); +- +- double inv_length = (c) ? (1.0 / sqrt(w(0) * w(0) + w(2) * w(2))) : (1.0 / sqrt(w(1) * w(1) + w(2) * w(2))); +- +- u(0) = (c) ? -w(2) * inv_length : 0.0; +- u(1) = (c) ? 0.0 : w(2) * inv_length; +- u(2) = (c) ? w(0) * inv_length : -w(1) * inv_length; +- +- cross(w, u, v); +-} +- +-CUDAH void SymmetricEigensolver3x3::computeEigenvector2(int tid) +-{ +- if (norm_[tid] > 0.0) { +- MatrixDevice evec0(3, 1, offset_ * 3, eigenvectors_ + tid + i02_[tid] * offset_); +- MatrixDevice evec1(3, 1, offset_ * 3, eigenvectors_ + tid + offset_); +- MatrixDevice evec2(3, 1, offset_ * 3, eigenvectors_ + tid + i02_[tid + offset_] * offset_); +- +- cross(evec0, evec1, evec2); +- } else { +- eigenvectors_[tid + offset_ * 8] = 1.0; +- } +-} +-} +- +-#endif +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/VoxelGrid.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/VoxelGrid.h +deleted file mode 100644 +index a273b8d4..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/VoxelGrid.h ++++ /dev/null +@@ -1,163 +0,0 @@ +-#ifndef GPU_OCTREE_H_ +-#define GPU_OCTREE_H_ +- +-#include +-#include +-#include +-#include "common.h" +-#include "MatrixDevice.h" +-#include "MatrixHost.h" +-#include +-#include +- +-namespace gpu { +-class GVoxelGrid { +-public: +- GVoxelGrid(); +- +- GVoxelGrid(const GVoxelGrid &other); +- +- /* Set input points */ +- void setInput(float *x, float *y, float *z, int points_num); +- +- void setMinVoxelSize(int size); +- +- /* For each input point, search for voxels whose distance between their centroids and +- * the input point are less than radius. +- * Results of the search are stored into valid_points, starting_voxel_id, and voxel_id. +- * Valid points: the function return one or more voxels for these points. Other points +- * are considered as invalid. +- * Valid voxels: voxels returned from the search. +- * The number of valid points is stored in valid_points_num. +- * The total number of valid voxels. If the query of both point A and point B return +- * voxel X, then the number of valid voxels is 2, not 1. */ +- void radiusSearch(float *qx, float *qy, float *qz, int points_num, float radius, int max_nn, +- int **valid_points, int **starting_voxel_id, int **voxel_id, +- int *valid_voxel_num, int *valid_points_num); +- +- int getVoxelNum() const; +- +- float getMaxX() const; +- float getMaxY() const; +- float getMaxZ() const; +- +- float getMinX() const; +- float getMinY() const; +- float getMinZ() const; +- +- float getVoxelX() const; +- float getVoxelY() const; +- float getVoxelZ() const; +- +- int getMaxBX() const; +- int getMaxBY() const; +- int getMaxBZ() const; +- +- int getMinBX() const; +- int getMinBY() const; +- int getMinBZ() const; +- +- int getVgridX() const; +- int getVgridY() const; +- int getVgridZ() const; +- +- void setLeafSize(float voxel_x, float voxel_y, float voxel_z); +- +- /* Get the centroid list. */ +- double *getCentroidList() const; +- +- /* Get the covariance list. */ +- double *getCovarianceList() const; +- +- /* Get the pointer to the inverse covariances list. */ +- double *getInverseCovarianceList() const; +- +- int *getPointsPerVoxelList() const; +- +- /* Searching for the nearest point of each input query point. +- * Coordinates of query points are input by trans_x, trans_y, and trans_z. +- * The ith element of valid_distance array is 1 if the distance between +- * the ith input point and its nearest neighbor is less than or equal +- * to max_range. Otherwise, it is 0. +- * The ith element of min_distance array stores the distance between +- * the corresponding input point and its nearest neighbor. It is 0 if +- * the distance is larger than max_range. */ +- void nearestNeighborSearch(float *trans_x, float *trans_y, float *trans_z, int point_num, int *valid_distance, double *min_distance, float max_range); +- +- ~GVoxelGrid(); +-private: +- +- /* Construct the voxel grid and the build the octree. */ +- void initialize(); +- +- /* Compute centroids and covariances of voxels. */ +- void computeCentroidAndCovariance(); +- +- /* Find boundaries of input point cloud and compute +- * the number of necessary voxels as well as boundaries +- * measured in number of leaf size */ +- void findBoundaries(); +- +- /* Put points into voxels */ +- void scatterPointsToVoxelGrid(); +- +- /* Build octrees for nearest neighbor search. +- * Only used for searching one nearest neighbor point. +- * Cannot used for searching multiple nearest neighbors. */ +- void buildOctree(); +- +- +- /* A wrapper for exclusive scan using thrust. +- * Output sum of all elements is stored at sum. */ +- template +- void ExclusiveScan(T *input, int ele_num, T *sum); +- +- /* A wrapper for exclusive scan using thrust. +- * Output sum is not required. */ +- template +- void ExclusiveScan(T *input, int ele_num); +- +- /* Size of the octree in each level, +- * measured in number of tree nodes. */ +- typedef struct _OctreeGridSize { +- int size_x; +- int size_y; +- int size_z; +- } OctreeGridSize; +- +- //Coordinate of input points +- float *x_, *y_, *z_; +- int points_num_; +- double *centroid_; // List of 3x1 double vector +- double *covariance_; // List of 3x3 double matrix +- double *inverse_covariance_; // List of 3x3 double matrix +- int *points_per_voxel_; +- +- int voxel_num_; // Number of voxels +- float max_x_, max_y_, max_z_; // Upper bounds of the grid (maximum coordinate) +- float min_x_, min_y_, min_z_; // Lower bounds of the grid (minimum coordinate) +- float voxel_x_, voxel_y_, voxel_z_; // Leaf size, a.k.a, size of each voxel +- +- int max_b_x_, max_b_y_, max_b_z_; // Upper bounds of the grid, measured in number of voxels +- int min_b_x_, min_b_y_, min_b_z_; // Lower bounds of the grid, measured in number of voxels +- int vgrid_x_, vgrid_y_, vgrid_z_; // Size of the voxel grid, measured in number of voxels +- int min_points_per_voxel_; +- +- int *starting_point_ids_; +- int *point_ids_; +- +- /* Centroids of octree nodes. Each element stores a list +- * of 3x1 matrices containing centroids of octree nodes. */ +- std::vector octree_centroids_; +- +- /* The number of points per octree node per level. */ +- std::vector octree_points_per_node_; +- +- /* The number of octree nodes per level in three dimensions. */ +- std::vector octree_grid_size_; +- +- bool is_copied_; +-}; +-} +- +-#endif +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/common.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/common.h +deleted file mode 100644 +index 2d14d2c4..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/common.h ++++ /dev/null +@@ -1,23 +0,0 @@ +-#ifndef GPU_COMMON_H_ +-#define GPU_COMMON_H_ +- +-#include +-#include +- +-#define CUDAH __forceinline__ __host__ __device__ +-#define BLOCK_SIZE_X 1024 +-#define BLOCK_SIZE_X2 512 +-#define BLOCK_SIZE_X3 256 +- +-#define BLOCK_X 16 +-#define BLOCK_Y 16 +-#define BLOCK_Z 4 +- +-#define SHARED_MEM_SIZE 3072 +-#endif +- +-// This is the temploary patch for CUDA9 build problem +-#if ( __CUDACC_VER_MAJOR__ >=9 ) +-#undef __CUDACC_VER__ +-#define __CUDACC_VER__ 90000 +-#endif +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/debug.h b/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/debug.h +deleted file mode 100644 +index bdc41dd2..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/include/ndt_gpu/debug.h ++++ /dev/null +@@ -1,24 +0,0 @@ +-#ifndef GDEBUG_H_ +-#define GDEBUG_H_ +- +-#include +-#include +-#include +-#include +-#include +-#include +- +-inline void gassert(cudaError_t err_code, const char *file, int line) +-{ +- if (err_code != cudaSuccess) { +- fprintf(stderr, "Error: %s %s %d\n", cudaGetErrorString(err_code), file, line); +- cudaDeviceReset(); +- exit(EXIT_FAILURE); +- } +-} +- +-#define checkCudaErrors(err_code) gassert(err_code, __FILE__, __LINE__) +- +-#define timeDiff(start, end) ((end.tv_sec - start.tv_sec) * 1000000 + end.tv_usec - start.tv_usec) +- +-#endif +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/package.xml b/autoware.ai/src/autoware/core_perception/ndt_gpu/package.xml +deleted file mode 100644 +index 3f5cad32..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/package.xml ++++ /dev/null +@@ -1,17 +0,0 @@ +- +- +- ndt_gpu +- 1.12.0 +- The ndt_gpu package +- Yuki Kitsukawa +- Anh Viet Nguyen +- Apache 2 +- +- autoware_build_flags +- catkin +- +- libpcl-all-dev +- +- libpcl-all +- rubis_lib +- +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixDevice.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixDevice.cu +deleted file mode 100644 +index 6e84a466..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixDevice.cu ++++ /dev/null +@@ -1,82 +0,0 @@ +-#include "ndt_gpu/MatrixDevice.h" +-#include "ndt_gpu/debug.h" +-#include "rubis_lib/sched.hpp" +- +-namespace gpu { +-MatrixDevice::MatrixDevice(int rows, int cols) { +- rows_ = rows; +- cols_ = cols; +- offset_ = 1; +- fr_ = true; +- buffer_ = NULL; +- memAllocId = 0; +- memFreeId = 0; +-} +- +-void MatrixDevice::memAlloc() +-{ +- if (buffer_ != NULL && fr_) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(buffer_)); +- rubis::sched::yield_gpu("1_free"); +- buffer_ = NULL; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&buffer_, sizeof(double) * rows_ * cols_ * offset_)); +- rubis::sched::yield_gpu("2_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(buffer_, 0, sizeof(double) * rows_ * cols_ * offset_)); +- rubis::sched::yield_gpu("3_cudaMemset"); +- +- checkCudaErrors(cudaDeviceSynchronize()); +- fr_ = true; +-} +- +-void MatrixDevice::memAlloc_free() +-{ +- if (buffer_ != NULL && fr_) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(buffer_)); +- rubis::sched::yield_gpu("4_free"); +- buffer_ = NULL; +- } +-} +- +-void MatrixDevice::memAlloc_malloc() +-{ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&buffer_, sizeof(double) * rows_ * cols_ * offset_)); +- rubis::sched::yield_gpu("5_cudaMalloc"); +-} +- +-void MatrixDevice::memAlloc_memset() +-{ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(buffer_, 0, sizeof(double) * rows_ * cols_ * offset_)); +- rubis::sched::yield_gpu("6_cudaMemset"); +- +- fr_ = true; +-} +- +-void MatrixDevice::memFree() +-{ +- if (fr_) { +- if (buffer_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(buffer_)); +- rubis::sched::yield_gpu("7_free"); +- buffer_ = NULL; +- } +- } +-} +- +- +-SquareMatrixDevice::SquareMatrixDevice(int size) : +- MatrixDevice(size, size) +-{ +- +-} +- +-} +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixHost.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixHost.cu +deleted file mode 100644 +index 65eaefac..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/MatrixHost.cu ++++ /dev/null +@@ -1,181 +0,0 @@ +-#include "ndt_gpu/MatrixHost.h" +-#include "ndt_gpu/debug.h" +-#include +-#include +-#include +-#include "rubis_lib/sched.hpp" +- +-namespace gpu { +- +-MatrixHost::MatrixHost() +-{ +- fr_ = false; +-} +- +-MatrixHost::MatrixHost(int rows, int cols) { +- rows_ = rows; +- cols_ = cols; +- offset_ = 1; +- +- buffer_ = (double*)malloc(sizeof(double) * rows_ * cols_ * offset_); +- memset(buffer_, 0, sizeof(double) * rows_ * cols_ * offset_); +- fr_ = true; +-} +- +-MatrixHost::MatrixHost(int rows, int cols, int offset, double *buffer) +-{ +- rows_ = rows; +- cols_ = cols; +- offset_ = offset; +- buffer_ = buffer; +- fr_ = false; +-} +- +-MatrixHost::MatrixHost(const MatrixHost& other) { +- rows_ = other.rows_; +- cols_ = other.cols_; +- offset_ = other.offset_; +- fr_ = other.fr_; +- +- if (fr_) { +- buffer_ = (double*)malloc(sizeof(double) * rows_ * cols_ * offset_); +- memcpy(buffer_, other.buffer_, sizeof(double) * rows_ * cols_ * offset_); +- } else { +- buffer_ = other.buffer_; +- } +-} +- +-extern "C" __global__ void copyMatrixDevToDev(MatrixDevice input, MatrixDevice output) { +- int row = threadIdx.x; +- int col = threadIdx.y; +- int rows_num = input.rows(); +- int cols_num = input.cols(); +- +- if (row < rows_num && col < cols_num) +- output(row, col) = input(row, col); +-} +- +-bool MatrixHost::moveToGpu(MatrixDevice output) { +- if (rows_ != output.rows() || cols_ != output.cols()) +- return false; +- +- if (offset_ == output.offset()) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(output.buffer(), buffer_, sizeof(double) * rows_ * cols_ * offset_, cudaMemcpyHostToDevice)); +- rubis::sched::yield_gpu("8_htod"); +- return true; +- } +- else { +- double *tmp; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&tmp, sizeof(double) * rows_ * cols_ * offset_)); +- rubis::sched::yield_gpu("9_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(tmp, buffer_, sizeof(double) * rows_ * cols_ * offset_, cudaMemcpyHostToDevice)); +- rubis::sched::yield_gpu("10_htod"); +- +- MatrixDevice tmp_output(rows_, cols_, offset_, tmp); +- +- dim3 block_x(rows_, cols_, 1); +- dim3 grid_x(1, 1, 1); +- +- rubis::sched::request_gpu(); +- copyMatrixDevToDev<<>>(tmp_output, output); +- rubis::sched::yield_gpu("11_copyMatrixDevToDev"); +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(tmp)); +- rubis::sched::yield_gpu("12_free"); +- +- return true; +- } +-} +- +-bool MatrixHost::moveToHost(MatrixDevice input) { +- if (rows_ != input.rows() || cols_ != input.cols()) +- return false; +- +- if (offset_ == input.offset()) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(buffer_, input.buffer(), sizeof(double) * rows_ * cols_ * offset_, cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("13_dtoh"); +- return true; +- } +- else { +- double *tmp; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&tmp, sizeof(double) * rows_ * cols_ * offset_)); +- rubis::sched::yield_gpu("14_cudaMalloc"); +- +- MatrixDevice tmp_output(rows_, cols_, offset_, tmp); +- +- dim3 block_x(rows_, cols_, 1); +- dim3 grid_x(1, 1, 1); +- +- rubis::sched::request_gpu(); +- copyMatrixDevToDev << > >(input, tmp_output); +- rubis::sched::yield_gpu("15_copyMatrixDevToDev"); +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(buffer_, tmp, sizeof(double) * rows_ * cols_ * offset_, cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("16_dtoh"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(tmp)); +- rubis::sched::yield_gpu("17_free"); +- +- return true; +- } +-} +- +-MatrixHost &MatrixHost::operator=(const MatrixHost &other) +-{ +- rows_ = other.rows_; +- cols_ = other.cols_; +- offset_ = other.offset_; +- fr_ = other.fr_; +- +- if (fr_) { +- buffer_ = (double*)malloc(sizeof(double) * rows_ * cols_ * offset_); +- memcpy(buffer_, other.buffer_, sizeof(double) * rows_ * cols_ * offset_); +- } else { +- buffer_ = other.buffer_; +- } +- +- return *this; +-} +- +-void MatrixHost::debug() +-{ +- for (int i = 0; i < rows_; i++) { +- for (int j = 0; j < cols_; j++) { +- std::cout << buffer_[(i * cols_ + j) * offset_] << " "; +- } +- +- std::cout << std::endl; +- } +- +- std::cout << std::endl; +-} +- +-MatrixHost::~MatrixHost() +-{ +- if (fr_) +- free(buffer_); +-} +- +- +-SquareMatrixHost::SquareMatrixHost(int size) : +- MatrixHost(size, size) +-{ +- +-} +- +-} +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/NormalDistributionsTransform.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/NormalDistributionsTransform.cu +deleted file mode 100644 +index 005268d4..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/NormalDistributionsTransform.cu ++++ /dev/null +@@ -1,1918 +0,0 @@ +-#include "ndt_gpu/NormalDistributionsTransform.h" +-#include "ndt_gpu/debug.h" +-#include +-#include +-#include +-#include "rubis_lib/sched.hpp" +- +-using namespace rubis::sched; +- +-namespace gpu { +- +-GNormalDistributionsTransform::GNormalDistributionsTransform() +-{ +- //GRegistration::GRegistration(); +- +- gauss_d1_ = gauss_d2_ = 0; +- outlier_ratio_ = 0.55; +- step_size_ = 0.1; +- resolution_ = 1.0f; +- trans_probability_ = 0; +- +- double gauss_c1, gauss_c2, gauss_d3; +- +- // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009] +- gauss_c1 = 10.0 * (1 - outlier_ratio_); +- gauss_c2 = outlier_ratio_ / pow (resolution_, 3); +- gauss_d3 = -log (gauss_c2); +- gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3; +- gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_); +- +- transformation_epsilon_ = 0.1; +- max_iterations_ = 35; +- +- j_ang_ = MatrixHost(24, 1); +- +- h_ang_ = MatrixHost(45, 1); +- +- dj_ang_ = MatrixDevice(24, 1); +- +- dh_ang_ = MatrixDevice(45, 1); +- +- real_iterations_ = 0; +- +-} +- +-GNormalDistributionsTransform::GNormalDistributionsTransform(const GNormalDistributionsTransform &other) +-{ +- gauss_d1_ = other.gauss_d1_; +- gauss_d2_ = other.gauss_d2_; +- +- outlier_ratio_ = other.outlier_ratio_; +- +- j_ang_ = other.j_ang_; +- h_ang_ = other.h_ang_; +- dj_ang_ = other.dj_ang_; +- dh_ang_ = other.dh_ang_; +- +- step_size_ = other.step_size_; +- resolution_ = other.resolution_; +- trans_probability_ = other.trans_probability_; +- real_iterations_ = other.real_iterations_; +- +- voxel_grid_ = other.voxel_grid_; +- +-} +- +-GNormalDistributionsTransform::~GNormalDistributionsTransform() +-{ +- dj_ang_.memFree(); +- dh_ang_.memFree(); +- +-} +- +-void GNormalDistributionsTransform::setStepSize(double step_size) +-{ +- step_size_ = step_size; +-} +- +-void GNormalDistributionsTransform::setResolution(float resolution) +-{ +- resolution_ = resolution; +-} +- +-void GNormalDistributionsTransform::setOutlierRatio(double olr) +-{ +- outlier_ratio_ = olr; +-} +- +-double GNormalDistributionsTransform::getStepSize() const +-{ +- return step_size_; +-} +- +-float GNormalDistributionsTransform::getResolution() const +-{ +- return resolution_; +-} +- +-double GNormalDistributionsTransform::getOutlierRatio() const +-{ +- return outlier_ratio_; +-} +- +-double GNormalDistributionsTransform::getTransformationProbability() const +-{ +- return trans_probability_; +-} +- +-int GNormalDistributionsTransform::getRealIterations() +-{ +- return real_iterations_; +-} +- +-double GNormalDistributionsTransform::auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu) +-{ +- return (f_a - f_0 - mu * g_0 * a); +-} +- +-double GNormalDistributionsTransform::auxilaryFunction_dPsiMT(double g_a, double g_0, double mu) +-{ +- return (g_a - mu * g_0); +-} +- +-void GNormalDistributionsTransform::setInputTarget(pcl::PointCloud::Ptr input) +-{ +- // Copy input map data from the host memory to the GPU memory +- GRegistration::setInputTarget(input); +- +- // Build the voxel grid +- if (target_points_number_ != 0) { +- voxel_grid_.setLeafSize(resolution_, resolution_, resolution_); +- voxel_grid_.setInput(target_x_, target_y_, target_z_, target_points_number_); +- } +- printf("complete set Input Target\n"); +-} +- +-void GNormalDistributionsTransform::setInputTarget(pcl::PointCloud::Ptr input) +-{ +- // Copy input map data from the host memory to the GPU memory +- GRegistration::setInputTarget(input); +- +- // Build the voxel grid +- if (target_points_number_ != 0) { +- voxel_grid_.setLeafSize(resolution_, resolution_, resolution_); +- voxel_grid_.setInput(target_x_, target_y_, target_z_, target_points_number_); +- } +- printf("complete set Input Target\n"); +-} +- +-void GNormalDistributionsTransform::computeTransformation(const Eigen::Matrix &guess) +-{ +- if (dj_ang_.isEmpty()) { +- dj_ang_.memAlloc(); +- } +- +- if (dh_ang_.isEmpty()) { +- dh_ang_.memAlloc(); +- } +- +- nr_iterations_ = 0; +- converged_ = false; +- +- double gauss_c1, gauss_c2, gauss_d3; +- +- gauss_c1 = 10 * ( 1 - outlier_ratio_); +- gauss_c2 = outlier_ratio_ / pow(resolution_, 3); +- gauss_d3 = - log(gauss_c2); +- gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3; +- gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3) / gauss_d1_); +- +- if (guess != Eigen::Matrix4f::Identity()) { +- final_transformation_ = guess; +- +- transformPointCloud(x_, y_, z_, trans_x_, trans_y_, trans_z_, points_number_, guess); +- } +- +- Eigen::Transform eig_transformation; +- eig_transformation.matrix() = final_transformation_; +- +- Eigen::Matrix p, delta_p, score_gradient; +- Eigen::Vector3f init_translation = eig_transformation.translation(); +- Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2); +- +- p << init_translation(0), init_translation(1), init_translation(2), init_rotation(0), init_rotation(1), init_rotation(2); +- +- Eigen::Matrix hessian; +- +- double score = 0; +- double delta_p_norm; +- +- score = computeDerivatives(score_gradient, hessian, trans_x_, trans_y_, trans_z_, points_number_, p); +- +- int loop_time = 0; +- +- while (!converged_) { +- previous_transformation_ = transformation_; +- +- Eigen::JacobiSVD> sv(hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); +- +- delta_p = sv.solve(-score_gradient); +- +- delta_p_norm = delta_p.norm(); +- +- if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) { +- +- trans_probability_ = score / static_cast(points_number_); +- converged_ = delta_p_norm == delta_p_norm; +- return; +- } +- +- delta_p.normalize(); +- delta_p_norm = computeStepLengthMT(p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, trans_x_, trans_y_, trans_z_, points_number_); +- +- delta_p *= delta_p_norm; +- +- Eigen::Translation translation(static_cast(delta_p(0)), static_cast(delta_p(1)), static_cast(delta_p(2))); +- Eigen::AngleAxis tmp1(static_cast(delta_p(3)), Eigen::Vector3f::UnitX()); +- Eigen::AngleAxis tmp2(static_cast(delta_p(4)), Eigen::Vector3f::UnitY()); +- Eigen::AngleAxis tmp3(static_cast(delta_p(5)), Eigen::Vector3f::UnitZ()); +- Eigen::AngleAxis tmp4(tmp1 * tmp2 * tmp3); +- +- transformation_ = (translation * tmp4).matrix(); +- +- p = p + delta_p; +- +- //Not update visualizer +- if (nr_iterations_ > max_iterations_ || (nr_iterations_ && (std::fabs(delta_p_norm) < transformation_epsilon_))) +- converged_ = true; +- +- nr_iterations_++; +- +- loop_time++; +- } +- +- trans_probability_ = score / static_cast(points_number_); +-} +- +-/* First step of computing point gradients */ +-__global__ void computePointGradients0(float *x, float *y, float *z, int points_num, +- int *valid_points, int valid_points_num, +- double *dj_ang, +- double *pg00, double *pg11, double *pg22, +- double *pg13, double *pg23, double *pg04, double *pg14) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- __shared__ double j_ang[12]; +- +- +- if (threadIdx.x < 12) { +- j_ang[threadIdx.x] = dj_ang[threadIdx.x]; +- } +- +- __syncthreads(); +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- +- //Orignal coordinates +- double o_x = static_cast(x[pid]); +- double o_y = static_cast(y[pid]); +- double o_z = static_cast(z[pid]); +- +- //Set the 3x3 block start from (0, 0) to identity matrix +- pg00[i] = 1; +- pg11[i] = 1; +- pg22[i] = 1; +- +- //Compute point derivatives +- pg13[i] = o_x * j_ang[0] + o_y * j_ang[1] + o_z * j_ang[2]; +- pg23[i] = o_x * j_ang[3] + o_y * j_ang[4] + o_z * j_ang[5]; +- pg04[i] = o_x * j_ang[6] + o_y * j_ang[7] + o_z * j_ang[8]; +- pg14[i] = o_x * j_ang[9] + o_y * j_ang[10] + o_z * j_ang[11]; +- } +-} +- +-/* Second step of computing point gradients */ +-__global__ void computePointGradients1(float *x, float *y, float *z, int points_num, +- int *valid_points, int valid_points_num, +- double *dj_ang, +- double *pg24, double *pg05, double *pg15, double *pg25) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- __shared__ double j_ang[12]; +- +- +- if (threadIdx.x < 12) { +- j_ang[threadIdx.x] = dj_ang[threadIdx.x + 12]; +- } +- +- __syncthreads(); +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- +- //Orignal coordinates +- double o_x = static_cast(x[pid]); +- double o_y = static_cast(y[pid]); +- double o_z = static_cast(z[pid]); +- +- //Compute point derivatives +- +- pg24[i] = o_x * j_ang[0] + o_y * j_ang[1] + o_z * j_ang[2]; +- pg05[i] = o_x * j_ang[3] + o_y * j_ang[4] + o_z * j_ang[5]; +- pg15[i] = o_x * j_ang[6] + o_y * j_ang[7] + o_z * j_ang[8]; +- pg25[i] = o_x * j_ang[9] + o_y * j_ang[10] + o_z * j_ang[11]; +- } +-} +- +- +-/* First step of computing point hessians */ +-__global__ void computePointHessian0(float *x, float *y, float *z, int points_num, +- int *valid_points, int valid_points_num, +- double *dh_ang, +- double *ph93, double *ph103, double *ph113, +- double *ph123, double *ph94, double *ph133, +- double *ph104, double *ph143, double *ph114, +- double *ph153, double *ph95, double *ph163, +- double *ph105, double *ph173, double *ph115) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- __shared__ double h_ang[18]; +- +- if (threadIdx.x < 18) { +- h_ang[threadIdx.x] = dh_ang[threadIdx.x]; +- } +- +- __syncthreads(); +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- +- //Orignal coordinates +- double o_x = static_cast(x[pid]); +- double o_y = static_cast(y[pid]); +- double o_z = static_cast(z[pid]); +- +- +- ph93[i] = 0; +- ph103[i] = o_x * h_ang[0] + o_y * h_ang[1] + o_z * h_ang[2]; +- ph113[i] = o_x * h_ang[3] + o_y * h_ang[4] + o_z * h_ang[5]; +- +- ph123[i] = ph94[i] = 0; +- ph133[i] = ph104[i] = o_x * h_ang[6] + o_y * h_ang[7] + o_z * h_ang[8]; +- ph143[i] = ph114[i] = o_x * h_ang[9] + o_y * h_ang[10] + o_z * h_ang[11]; +- +- ph153[i] = ph95[i] = 0; +- ph163[i] = ph105[i] = o_x * h_ang[12] + o_y * h_ang[13] + o_z * h_ang[14]; +- ph173[i] = ph115[i] = o_x * h_ang[15] + o_y * h_ang[16] + o_z * h_ang[17]; +- +- } +-} +- +-__global__ void computePointHessian1(float *x, float *y, float *z, int points_num, +- int *valid_points, int valid_points_num, +- double *dh_ang, +- double *ph124, double *ph134, double *ph144, +- double *ph154, double *ph125, double *ph164, +- double *ph135, double *ph174, double *ph145) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- __shared__ double h_ang[18]; +- +- if (threadIdx.x < 18) { +- h_ang[threadIdx.x] = dh_ang[18 + threadIdx.x]; +- } +- +- __syncthreads(); +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- +- //Orignal coordinates +- double o_x = static_cast(x[pid]); +- double o_y = static_cast(y[pid]); +- double o_z = static_cast(z[pid]); +- +- ph124[i] = o_x * h_ang[0] + o_y * h_ang[1] + o_z * h_ang[2]; +- ph134[i] = o_x * h_ang[3] + o_y * h_ang[4] + o_z * h_ang[5]; +- ph144[i] = o_x * h_ang[6] + o_y * h_ang[7] + o_z * h_ang[8]; +- +- ph154[i] = ph125[i] = o_x * h_ang[9] + o_y * h_ang[10] + o_z * h_ang[11]; +- ph164[i] = ph135[i] = o_x * h_ang[12] + o_y * h_ang[13] + o_z * h_ang[14]; +- ph174[i] = ph145[i] = o_x * h_ang[15] + o_y * h_ang[16] + o_z * h_ang[17]; +- } +-} +- +-__global__ void computePointHessian2(float *x, float *y, float *z, int points_num, +- int *valid_points, int valid_points_num, +- double *dh_ang, +- double *ph155, double *ph165, double *ph175) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- __shared__ double h_ang[9]; +- +- if (threadIdx.x < 9) { +- h_ang[threadIdx.x] = dh_ang[36 + threadIdx.x]; +- } +- +- __syncthreads(); +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- +- //Orignal coordinates +- double o_x = static_cast(x[pid]); +- double o_y = static_cast(y[pid]); +- double o_z = static_cast(z[pid]); +- +- ph155[i] = o_x * h_ang[0] + o_y * h_ang[1] + o_z * h_ang[2]; +- ph165[i] = o_x * h_ang[3] + o_y * h_ang[4] + o_z * h_ang[5]; +- ph175[i] = o_x * h_ang[6] + o_y * h_ang[7] + o_z * h_ang[8]; +- +- } +-} +- +-/* compute score_inc list for input points. +- * The final score_inc is calculated by a reduction sum +- * on this score_inc list. */ +-__global__ void computeScoreList(int *starting_voxel_id, int *voxel_id, int valid_points_num, +- double *e_x_cov_x, double gauss_d1, double *score) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = id; i < valid_points_num; i += stride) { +- +- double score_inc = 0; +- +- for (int vid = starting_voxel_id[i]; vid < starting_voxel_id[i + 1]; vid++) { +- double tmp_ex = e_x_cov_x[vid]; +- +- score_inc += (tmp_ex > 1 || tmp_ex < 0 || tmp_ex != tmp_ex) ? 0 : -gauss_d1 * tmp_ex; +- } +- +- score[i] = score_inc; +- } +-} +- +-/* First step to compute score gradient list for input points */ +-__global__ void computeScoreGradientList(float *trans_x, float *trans_y, float *trans_z, +- int *valid_points, +- int *starting_voxel_id, int *voxel_id, int valid_points_num, +- double *centroid_x, double *centroid_y, double *centroid_z, +- int voxel_num, double *e_x_cov_x, +- double *cov_dxd_pi, double gauss_d1, int valid_voxel_num, +- double *score_gradients) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- int col = blockIdx.y; +- +- if (col < 6) { +- double *sg = score_gradients + col * valid_points_num; +- double *cov_dxd_pi_mat0 = cov_dxd_pi + col * valid_voxel_num; +- double *cov_dxd_pi_mat1 = cov_dxd_pi_mat0 + 6 * valid_voxel_num; +- double *cov_dxd_pi_mat2 = cov_dxd_pi_mat1 + 6 * valid_voxel_num; +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- double d_x = static_cast(trans_x[pid]); +- double d_y = static_cast(trans_y[pid]); +- double d_z = static_cast(trans_z[pid]); +- +- double tmp_sg = 0.0; +- +- for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) { +- int vid = voxel_id[j]; +- double tmp_ex = e_x_cov_x[j]; +- +- if (!(tmp_ex > 1 || tmp_ex < 0 || tmp_ex != tmp_ex)) { +- tmp_ex *= gauss_d1; +- +- tmp_sg += ((d_x - centroid_x[vid]) * cov_dxd_pi_mat0[j] + (d_y - centroid_y[vid]) * cov_dxd_pi_mat1[j] + (d_z - centroid_z[vid]) * cov_dxd_pi_mat2[j]) * tmp_ex; +- } +- } +- +- sg[i] = tmp_sg; +- } +- } +-} +- +-/* Intermediate step to compute e_x_cov_x */ +-__global__ void computeExCovX(float *trans_x, float *trans_y, float *trans_z, int *valid_points, +- int *starting_voxel_id, int *voxel_id, int valid_points_num, +- double *centr_x, double *centr_y, double *centr_z, +- double gauss_d1, double gauss_d2, +- double *e_x_cov_x, +- double *icov00, double *icov01, double *icov02, +- double *icov10, double *icov11, double *icov12, +- double *icov20, double *icov21, double *icov22) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- double d_x = static_cast(trans_x[pid]); +- double d_y = static_cast(trans_y[pid]); +- double d_z = static_cast(trans_z[pid]); +- double t_x, t_y, t_z; +- +- +- for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) { +- int vid = voxel_id[j]; +- +- t_x = d_x - centr_x[vid]; +- t_y = d_y - centr_y[vid]; +- t_z = d_z - centr_z[vid]; +- +- e_x_cov_x[j] = exp(-gauss_d2 * ((t_x * icov00[vid] + t_y * icov01[vid] + t_z * icov02[vid]) * t_x +- + ((t_x * icov10[vid] + t_y * icov11[vid] + t_z * icov12[vid]) * t_y) +- + ((t_x * icov20[vid] + t_y * icov21[vid] + t_z * icov22[vid]) * t_z)) / 2.0); +- } +- } +-} +- +-/* update e_x_cov_x - Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] */ +-__global__ void updateExCovX(double *e_x_cov_x, double gauss_d2, int valid_voxel_num) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = id; i < valid_voxel_num; i += stride) { +- e_x_cov_x[i] *= gauss_d2; +- } +-} +- +-/* compute cov_dxd_pi as reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]*/ +-__global__ void computeCovDxdPi(int *valid_points, int *starting_voxel_id, int *voxel_id, int valid_points_num, +- double *inverse_covariance, int voxel_num, +- double gauss_d1, double gauss_d2, double *point_gradients, +- double *cov_dxd_pi, int valid_voxel_num) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- int row = blockIdx.y; +- int col = blockIdx.z; +- +- if (row < 3 && col < 6) { +- double *icov0 = inverse_covariance + row * 3 * voxel_num; +- double *icov1 = icov0 + voxel_num; +- double *icov2 = icov1 + voxel_num; +- double *cov_dxd_pi_tmp = cov_dxd_pi + (row * 6 + col) * valid_voxel_num; +- double *pg_tmp0 = point_gradients + col * valid_points_num; +- double *pg_tmp1 = pg_tmp0 + 6 * valid_points_num; +- double *pg_tmp2 = pg_tmp1 + 6 * valid_points_num; +- +- for (int i = id; i < valid_points_num; i += stride) { +- double pg0 = pg_tmp0[i]; +- double pg1 = pg_tmp1[i]; +- double pg2 = pg_tmp2[i]; +- +- for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) { +- int vid = voxel_id[j]; +- +- cov_dxd_pi_tmp[j] = icov0[vid] * pg0 + icov1[vid] * pg1 + icov2[vid] * pg2; +- } +- } +- } +-} +- +- +-/* First step to compute hessian list for input points */ +-__global__ void computeHessianListS0(float *trans_x, float *trans_y, float *trans_z, +- int *valid_points, +- int *starting_voxel_id, int *voxel_id, int valid_points_num, +- double *centroid_x, double *centroid_y, double *centroid_z, +- double *icov00, double *icov01, double *icov02, +- double *icov10, double *icov11, double *icov12, +- double *icov20, double *icov21, double *icov22, +- double *point_gradients, +- double *tmp_hessian, +- int valid_voxel_num) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- int col = blockIdx.y; +- +- if (col < 6) { +- double *tmp_pg0 = point_gradients + col * valid_points_num; +- double *tmp_pg1 = tmp_pg0 + 6 * valid_points_num; +- double *tmp_pg2 = tmp_pg1 + 6 * valid_points_num; +- double *tmp_h = tmp_hessian + col * valid_voxel_num; +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- double d_x = static_cast(trans_x[pid]); +- double d_y = static_cast(trans_y[pid]); +- double d_z = static_cast(trans_z[pid]); +- +- double pg0 = tmp_pg0[i]; +- double pg1 = tmp_pg1[i]; +- double pg2 = tmp_pg2[i]; +- +- for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) { +- int vid = voxel_id[j]; +- +- tmp_h[j] = (d_x - centroid_x[vid]) * (icov00[vid] * pg0 + icov01[vid] * pg1 + icov02[vid] * pg2) +- + (d_y - centroid_y[vid]) * (icov10[vid] * pg0 + icov11[vid] * pg1 + icov12[vid] * pg2) +- + (d_z - centroid_z[vid]) * (icov20[vid] * pg0 + icov21[vid] * pg1 + icov22[vid] * pg2); +- } +- } +- } +-} +- +-/* Fourth step to compute hessian list */ +-__global__ void computeHessianListS1(float *trans_x, float *trans_y, float *trans_z, +- int *valid_points, +- int *starting_voxel_id, int *voxel_id, int valid_points_num, +- double *centroid_x, double *centroid_y, double *centroid_z, +- double gauss_d1, double gauss_d2, double *hessians, +- double *e_x_cov_x, double *tmp_hessian, double *cov_dxd_pi, +- double *point_gradients, +- int valid_voxel_num) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- int row = blockIdx.y; +- int col = blockIdx.z; +- +- if (row < 6 && col < 6) { +- double *cov_dxd_pi_mat0 = cov_dxd_pi + row * valid_voxel_num; +- double *cov_dxd_pi_mat1 = cov_dxd_pi_mat0 + 6 * valid_voxel_num; +- double *cov_dxd_pi_mat2 = cov_dxd_pi_mat1 + 6 * valid_voxel_num; +- double *tmp_h = tmp_hessian + col * valid_voxel_num; +- double *h = hessians + (row * 6 + col) * valid_points_num; +- double *tmp_pg0 = point_gradients + col * valid_points_num; +- double *tmp_pg1 = tmp_pg0 + 6 * valid_points_num; +- double *tmp_pg2 = tmp_pg1 + 6 * valid_points_num; +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- double d_x = static_cast(trans_x[pid]); +- double d_y = static_cast(trans_y[pid]); +- double d_z = static_cast(trans_z[pid]); +- +- double pg0 = tmp_pg0[i]; +- double pg1 = tmp_pg1[i]; +- double pg2 = tmp_pg2[i]; +- +- double final_hessian = 0.0; +- +- for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) { +- //Transformed coordinates +- int vid = voxel_id[j]; +- +- double tmp_ex = e_x_cov_x[j]; +- +- if (!(tmp_ex > 1 || tmp_ex < 0 || tmp_ex != tmp_ex)) { +- double cov_dxd0 = cov_dxd_pi_mat0[j]; +- double cov_dxd1 = cov_dxd_pi_mat1[j]; +- double cov_dxd2 = cov_dxd_pi_mat2[j]; +- +- tmp_ex *= gauss_d1; +- +- final_hessian += -gauss_d2 * ((d_x - centroid_x[vid]) * cov_dxd0 + (d_y - centroid_y[vid]) * cov_dxd1 + (d_z - centroid_z[vid]) * cov_dxd2) * tmp_h[j] * tmp_ex; +- final_hessian += (pg0 * cov_dxd0 + pg1 * cov_dxd1 + pg2 * cov_dxd2) * tmp_ex; +- } +- } +- +- h[i] = final_hessian; +- } +- } +-} +- +-__global__ void computeHessianListS2(float *trans_x, float *trans_y, float *trans_z, +- int *valid_points, +- int *starting_voxel_id, int *voxel_id, int valid_points_num, +- double *centroid_x, double *centroid_y, double *centroid_z, +- double gauss_d1, double *e_x_cov_x, +- double *icov00, double *icov01, double *icov02, +- double *icov10, double *icov11, double *icov12, +- double *icov20, double *icov21, double *icov22, +- double *point_hessians, double *hessians, +- int valid_voxel_num) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- int row = blockIdx.y; +- int col = blockIdx.z; +- +- if (row < 6 && col < 6) { +- double *h = hessians + (row * 6 + col) * valid_points_num; +- double *tmp_ph0 = point_hessians + ((3 * row) * 6 + col) * valid_points_num; +- double *tmp_ph1 = tmp_ph0 + 6 * valid_points_num; +- double *tmp_ph2 = tmp_ph1 + 6 * valid_points_num; +- +- for (int i = id; i < valid_points_num; i += stride) { +- int pid = valid_points[i]; +- double d_x = static_cast(trans_x[pid]); +- double d_y = static_cast(trans_y[pid]); +- double d_z = static_cast(trans_z[pid]); +- double ph0 = tmp_ph0[i]; +- double ph1 = tmp_ph1[i]; +- double ph2 = tmp_ph2[i]; +- +- double final_hessian = h[i]; +- +- for ( int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1]; j++) { +- //Transformed coordinates +- int vid = voxel_id[j]; +- double tmp_ex = e_x_cov_x[j]; +- +- if (!(tmp_ex > 1 || tmp_ex < 0 || tmp_ex != tmp_ex)) { +- tmp_ex *= gauss_d1; +- +- final_hessian += (d_x - centroid_x[vid]) * (icov00[vid] * ph0 + icov01[vid] * ph1 + icov02[vid] * ph2) * tmp_ex; +- final_hessian += (d_y - centroid_y[vid]) * (icov10[vid] * ph0 + icov11[vid] * ph1 + icov12[vid] * ph2) * tmp_ex; +- final_hessian += (d_z - centroid_z[vid]) * (icov20[vid] * ph0 + icov21[vid] * ph1 + icov22[vid] * ph2) * tmp_ex; +- +- } +- } +- +- h[i] = final_hessian; +- } +- } +-} +- +- +-/* Compute sum of a list of matrices */ +-__global__ void matrixSum(double *matrix_list, int full_size, int half_size, int rows, int cols, int offset) +-{ +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- int row = blockIdx.y; +- int col = blockIdx.z; +- +- for (int i = index; i < half_size && row < rows && col < cols; i += stride) { +- MatrixDevice left(rows, cols, offset, matrix_list + i); +- double *right_ptr = (i + half_size < full_size) ? matrix_list + i + half_size : NULL; +- MatrixDevice right(rows, cols, offset, right_ptr); +- +- if (right_ptr != NULL) { +- left(row, col) += right(row, col); +- } +- } +-} +- +-/* Compute sum of score_inc list */ +-__global__ void sumScore(double *score, int full_size, int half_size) +-{ +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = index; i < half_size; i += stride) { +- score[i] += (i + half_size < full_size) ? score[i + half_size] : 0; +- } +-} +- +- +-double GNormalDistributionsTransform::computeDerivatives(Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, +- float *trans_x, float *trans_y, float *trans_z, +- int points_num, Eigen::Matrix pose, bool compute_hessian) +-{ +- MatrixHost p(6, 1); +- +- for (int i = 0; i < 6; i++) { +- p(i) = pose(i, 0); +- } +- +- score_gradient.setZero (); +- hessian.setZero (); +- +- //Compute Angle Derivatives +- computeAngleDerivatives(p); +- +- //Radius Search +- int *valid_points, *voxel_id, *starting_voxel_id; +- int valid_voxel_num, valid_points_num; +- +- valid_points = voxel_id = starting_voxel_id = NULL; +- +- voxel_grid_.radiusSearch(trans_x, trans_y, trans_z, points_num, resolution_, INT_MAX, &valid_points, &starting_voxel_id, &voxel_id, &valid_voxel_num, &valid_points_num); +- +- double *covariance = voxel_grid_.getCovarianceList(); +- double *inverse_covariance = voxel_grid_.getInverseCovarianceList(); +- double *centroid = voxel_grid_.getCentroidList(); +- int *points_per_voxel = voxel_grid_.getPointsPerVoxelList(); +- int voxel_num = voxel_grid_.getVoxelNum(); +- +- if (valid_points_num == 0) +- return 0; +- +- //Update score gradient and hessian matrix +- +- double *gradients, *hessians, *point_gradients, *point_hessians, *score; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&gradients, sizeof(double) * valid_points_num * 6)); +- rubis::sched::yield_gpu("18_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&hessians, sizeof(double) * valid_points_num * 6 * 6)); +- rubis::sched::yield_gpu("19_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&point_gradients, sizeof(double) * valid_points_num * 3 * 6)); +- rubis::sched::yield_gpu("20_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&point_hessians, sizeof(double) * valid_points_num * 18 * 6)); +- rubis::sched::yield_gpu("21_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&score, sizeof(double) * valid_points_num)); +- rubis::sched::yield_gpu("22_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(gradients, 0, sizeof(double) * valid_points_num * 6)); +- rubis::sched::yield_gpu("23_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(hessians, 0, sizeof(double) * valid_points_num * 6 * 6)); +- rubis::sched::yield_gpu("24_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(point_gradients, 0, sizeof(double) * valid_points_num * 3 * 6)); +- rubis::sched::yield_gpu("25_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(point_hessians, 0, sizeof(double) * valid_points_num * 18 * 6)); +- rubis::sched::yield_gpu("26_cudaMemset"); +- +- int block_x = (valid_points_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : valid_points_num; +- +- int grid_x = (valid_points_num - 1) / block_x + 1; +- +- dim3 grid; +- +- +- rubis::sched::request_gpu(); +- computePointGradients0<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dj_ang_.buffer(), +- point_gradients, +- point_gradients + valid_points_num * 7, +- point_gradients + valid_points_num * 14, +- point_gradients + valid_points_num * 9, +- point_gradients + valid_points_num * 15, +- point_gradients + valid_points_num * 4, +- point_gradients + valid_points_num * 10); +- rubis::sched::yield_gpu("27_computePointGradients0"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computePointGradients1<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dj_ang_.buffer(), +- point_gradients + valid_points_num * 16, +- point_gradients + valid_points_num * 5, +- point_gradients + valid_points_num * 11, +- point_gradients + valid_points_num * 17); +- rubis::sched::yield_gpu("28_computePointGradients1"); +- checkCudaErrors(cudaGetLastError()); +- +- if (compute_hessian) { +- rubis::sched::request_gpu(); +- computePointHessian0<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dh_ang_.buffer(), +- point_hessians + valid_points_num * 57, +- point_hessians + valid_points_num * 63, point_hessians + valid_points_num * 69, +- point_hessians + valid_points_num * 75, point_hessians + valid_points_num * 58, point_hessians + valid_points_num * 81, +- point_hessians + valid_points_num * 64, point_hessians + valid_points_num * 87, point_hessians + valid_points_num * 70, +- point_hessians + valid_points_num * 93, point_hessians + valid_points_num * 59, point_hessians + valid_points_num * 99, +- point_hessians + valid_points_num * 65, point_hessians + valid_points_num * 105, point_hessians + valid_points_num * 71); +- rubis::sched::yield_gpu("29_computePointHessian0"); +- checkCudaErrors(cudaGetLastError()); +- +- +- rubis::sched::request_gpu(); +- computePointHessian1<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dh_ang_.buffer(), +- point_hessians + valid_points_num * 76, point_hessians + valid_points_num * 82, point_hessians + valid_points_num * 88, +- point_hessians + valid_points_num * 94, point_hessians + valid_points_num * 77, point_hessians + valid_points_num * 100, +- point_hessians + valid_points_num * 83, point_hessians + valid_points_num * 106, point_hessians + valid_points_num * 89); +- rubis::sched::yield_gpu("30_computePointHessian1"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computePointHessian2<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dh_ang_.buffer(), +- point_hessians + valid_points_num * 95, point_hessians + valid_points_num * 101, point_hessians + valid_points_num * 107); +- rubis::sched::yield_gpu("31_computePointHessian2"); +- checkCudaErrors(cudaGetLastError()); +- +- } +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- +- double *tmp_hessian; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&tmp_hessian, sizeof(double) * valid_voxel_num * 6)); +- rubis::sched::yield_gpu("32_cudaMalloc"); +- +- double *e_x_cov_x; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&e_x_cov_x, sizeof(double) * valid_voxel_num)); +- rubis::sched::yield_gpu("33_cudaMalloc"); +- +- double *cov_dxd_pi; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&cov_dxd_pi, sizeof(double) * valid_voxel_num * 3 * 6)); +- rubis::sched::yield_gpu("34_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- computeExCovX<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- gauss_d1_, gauss_d2_, +- e_x_cov_x, +- inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num, +- inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num, +- inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num); +- rubis::sched::yield_gpu("35_computeExCovX"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeScoreList<<>>(starting_voxel_id, voxel_id, valid_points_num, e_x_cov_x, gauss_d1_, score); +- rubis::sched::yield_gpu("36_computeScoreList"); +- +- checkCudaErrors(cudaGetLastError()); +- +- int block_x2 = (valid_voxel_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : valid_voxel_num; +- int grid_x2 = (valid_voxel_num - 1) / block_x2 + 1; +- +- rubis::sched::request_gpu(); +- updateExCovX<<>>(e_x_cov_x, gauss_d2_, valid_voxel_num); +- rubis::sched::yield_gpu("37_computeScoreList"); +- +- checkCudaErrors(cudaGetLastError()); +- +- grid.x = grid_x; +- grid.y = 3; +- grid.z = 6; +- +- rubis::sched::request_gpu(); +- computeCovDxdPi<<>>(valid_points, starting_voxel_id, voxel_id, valid_points_num, +- inverse_covariance, voxel_num, +- gauss_d1_, gauss_d2_, point_gradients, +- cov_dxd_pi, valid_voxel_num); +- rubis::sched::yield_gpu("38_computeCovDxdPi"); +- checkCudaErrors(cudaGetLastError()); +- +- grid.x = grid_x; +- grid.y = 6; +- grid.z = 1; +- +- rubis::sched::request_gpu(); +- computeScoreGradientList<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- voxel_num, e_x_cov_x, +- cov_dxd_pi, gauss_d1_, valid_voxel_num, gradients); +- rubis::sched::yield_gpu("39_computeCovDxdPi"); +- checkCudaErrors(cudaGetLastError()); +- +- if (compute_hessian) { +- +- grid.y = 6; +- grid.z = 1; +- +- rubis::sched::request_gpu(); +- computeHessianListS0<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num, +- inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num, +- inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num, +- point_gradients, +- tmp_hessian, valid_voxel_num); +- rubis::sched::yield_gpu("40_computeHessianListS0"); +- +- checkCudaErrors(cudaGetLastError()); +- grid.z = 6; +- +- rubis::sched::request_gpu(); +- computeHessianListS1<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- gauss_d1_, gauss_d2_, hessians, +- e_x_cov_x, tmp_hessian, cov_dxd_pi, +- point_gradients, +- valid_voxel_num); +- rubis::sched::yield_gpu("41_computeHessianListS1"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeHessianListS2<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- gauss_d1_, e_x_cov_x, +- inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num, +- inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num, +- inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num, +- point_hessians, hessians, valid_voxel_num); +- rubis::sched::yield_gpu("42_computeHessianListS2"); +- checkCudaErrors(cudaGetLastError()); +- +- } +- +- int full_size = valid_points_num; +- int half_size = (full_size - 1) / 2 + 1; +- +- while (full_size > 1) { +- block_x = (half_size > BLOCK_SIZE_X) ? BLOCK_SIZE_X : half_size; +- grid_x = (half_size - 1) / block_x + 1; +- +- grid.x = grid_x; +- grid.y = 1; +- grid.z = 6; +- +- rubis::sched::request_gpu_in_loop(GPU_SEG_LOOP_START); +- matrixSum<<>>(gradients, full_size, half_size, 1, 6, valid_points_num); +- rubis::sched::yield_gpu_in_loop(GPU_SEG_LOOP_START,"43_matrixSum"); +- +- checkCudaErrors(cudaGetLastError()); +- +- grid.y = 6; +- +- rubis::sched::request_gpu_in_loop(GPU_SEG_LOOP_MID); +- matrixSum<<>>(hessians, full_size, half_size, 6, 6, valid_points_num); +- rubis::sched::yield_gpu_in_loop(GPU_SEG_LOOP_MID, "44_matrixSum"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu_in_loop(GPU_SEG_LOOP_END); +- sumScore<<>>(score, full_size, half_size); +- rubis::sched::yield_gpu_in_loop(GPU_SEG_LOOP_END, "45_sumScore"); +- +- checkCudaErrors(cudaGetLastError()); +- +- full_size = half_size; +- half_size = (full_size - 1) / 2 + 1; +- } +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- MatrixDevice dgrad(1, 6, valid_points_num, gradients), dhess(6, 6, valid_points_num, hessians); +- MatrixHost hgrad(1, 6), hhess(6, 6); +- +- hgrad.moveToHost(dgrad); +- +- hhess.moveToHost(dhess); +- +- for (int i = 0; i < 6; i++) { +- score_gradient(i) = hgrad(i); +- } +- +- for (int i = 0; i < 6; i++) { +- for (int j = 0; j < 6; j++) { +- hessian(i, j) = hhess(i, j); +- } +- } +- +- double score_inc; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&score_inc, score, sizeof(double), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("46_dtoh"); +- +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(gradients)); +- rubis::sched::yield_gpu("47_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(hessians)); +- rubis::sched::yield_gpu("48_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(point_hessians)); +- rubis::sched::yield_gpu("49_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(point_gradients)); +- rubis::sched::yield_gpu("50_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(score)); +- rubis::sched::yield_gpu("51_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(tmp_hessian)); +- rubis::sched::yield_gpu("52_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(e_x_cov_x)); +- rubis::sched::yield_gpu("53_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(cov_dxd_pi)); +- rubis::sched::yield_gpu("54_free"); +- +- if (valid_points != NULL){ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_points)); +- rubis::sched::yield_gpu("55_free"); +- } +- +- if (voxel_id != NULL){ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(voxel_id)); +- rubis::sched::yield_gpu("56_free"); +- } +- +- if (starting_voxel_id != NULL){ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(starting_voxel_id)); +- rubis::sched::yield_gpu("57_free"); +- } +- +- +- +- return score_inc; +-} +- +-void GNormalDistributionsTransform::computeAngleDerivatives(MatrixHost pose, bool compute_hessian) +-{ +- double cx, cy, cz, sx, sy, sz; +- +- if (fabs(pose(3)) < 10e-5) { +- cx = 1.0; +- sx = 0.0; +- } else { +- cx = cos(pose(3)); +- sx = sin(pose(3)); +- } +- +- if (fabs(pose(4)) < 10e-5) { +- cy = 1.0; +- sy = 0.0; +- } else { +- cy = cos(pose(4)); +- sy = sin(pose(4)); +- } +- +- if (fabs(pose(5)) < 10e-5) { +- cz = 1.0; +- sz = 0.0; +- } else { +- cz = cos(pose(5)); +- sz = sin(pose(5)); +- } +- +- +- j_ang_(0) = -sx * sz + cx * sy * cz; +- j_ang_(1) = -sx * cz - cx * sy * sz; +- j_ang_(2) = -cx * cy; +- +- j_ang_(3) = cx * sz + sx * sy * cz; +- j_ang_(4) = cx * cz - sx * sy * sz; +- j_ang_(5) = -sx * cy; +- +- j_ang_(6) = -sy * cz; +- j_ang_(7) = sy * sz; +- j_ang_(8) = cy; +- +- j_ang_(9) = sx * cy * cz; +- j_ang_(10) = -sx * cy * sz; +- j_ang_(11) = sx * sy; +- +- j_ang_(12) = -cx * cy * cz; +- j_ang_(13) = cx * cy * sz; +- j_ang_(14) = -cx * sy; +- +- j_ang_(15) = -cy * sz; +- j_ang_(16) = -cy * cz; +- j_ang_(17) = 0; +- +- j_ang_(18) = cx * cz - sx * sy * sz; +- j_ang_(19) = -cx * sz - sx * sy * cz; +- j_ang_(20) = 0; +- +- j_ang_(21) = sx * cz + cx * sy * sz; +- j_ang_(22) = cx * sy * cz - sx * sz; +- j_ang_(23) = 0; +- +- j_ang_.moveToGpu(dj_ang_); +- +- if (compute_hessian) { +- +- h_ang_(0) = -cx * sz - sx * sy * cz; +- h_ang_(1) = -cx * cz + sx * sy * sz; +- h_ang_(2) = sx * cy; +- +- h_ang_(3) = -sx * sz + cx * sy * cz; +- h_ang_(4) = -cx * sy * sz - sx * cz; +- h_ang_(5) = -cx * cy; +- +- h_ang_(6) = cx * cy * cz; +- h_ang_(7) = -cx * cy * sz; +- h_ang_(8) = cx * sy; +- +- h_ang_(9) = sx * cy * cz; +- h_ang_(10) = -sx * cy * sz; +- h_ang_(11) = sx * sy; +- +- h_ang_(12) = -sx * cz - cx * sy * sz; +- h_ang_(13) = sx * sz - cx * sy * cz; +- h_ang_(14) = 0; +- +- h_ang_(15) = cx * cz - sx * sy * sz; +- h_ang_(16) = -sx * sy * cz - cx * sz; +- h_ang_(17) = 0; +- +- h_ang_(18) = -cy * cz; +- h_ang_(19) = cy * sz; +- h_ang_(20) = sy; +- +- h_ang_(21) = -sx * sy * cz; +- h_ang_(22) = sx * sy * sz; +- h_ang_(23) = sx * cy; +- +- h_ang_(24) = cx * sy * cz; +- h_ang_(25) = -cx * sy * sz; +- h_ang_(26) = -cx * cy; +- +- h_ang_(27) = sy * sz; +- h_ang_(28) = sy * cz; +- h_ang_(29) = 0; +- +- h_ang_(30) = -sx * cy * sz; +- h_ang_(31) = -sx * cy * cz; +- h_ang_(32) = 0; +- +- h_ang_(33) = cx * cy * sz; +- h_ang_(34) = cx * cy * cz; +- h_ang_(35) = 0; +- +- h_ang_(36) = -cy * cz; +- h_ang_(37) = cy * sz; +- h_ang_(38) = 0; +- +- h_ang_(39) = -cx * sz - sx * sy * cz; +- h_ang_(40) = -cx * cz + sx * sy * sz; +- h_ang_(41) = 0; +- +- h_ang_(42) = -sx * sz + cx * sy * cz; +- h_ang_(43) = -cx * sy * sz - sx * cz; +- h_ang_(44) = 0; +- +- h_ang_.moveToGpu(dh_ang_); +- } +- +-} +- +- +- +- +-__global__ void gpuTransform(float *in_x, float *in_y, float *in_z, +- float *trans_x, float *trans_y, float *trans_z, +- int point_num, MatrixDevice transform) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- float x, y, z; +- +- for (int i = idx; i < point_num; i += stride) { +- x = in_x[i]; +- y = in_y[i]; +- z = in_z[i]; +- trans_x[i] = transform(0, 0) * x + transform(0, 1) * y + transform(0, 2) * z + transform(0, 3); +- trans_y[i] = transform(1, 0) * x + transform(1, 1) * y + transform(1, 2) * z + transform(1, 3); +- trans_z[i] = transform(2, 0) * x + transform(2, 1) * y + transform(2, 2) * z + transform(2, 3); +- } +-} +- +-void GNormalDistributionsTransform::transformPointCloud(float *in_x, float *in_y, float *in_z, +- float *trans_x, float *trans_y, float *trans_z, +- int points_number, Eigen::Matrix transform) +-{ +- Eigen::Transform t(transform); +- +- MatrixHost htrans(3, 4); +- MatrixDevice dtrans(3, 4); +- +- +- dtrans.memAlloc(); +- +- for (int i = 0; i < 3; i++) { +- for (int j = 0; j < 4; j++) { +- htrans(i, j) = t(i, j); +- } +- } +- htrans.moveToGpu(dtrans); +- +- if (points_number > 0) { +- int block_x = (points_number <= BLOCK_SIZE_X) ? points_number : BLOCK_SIZE_X; +- int grid_x = (points_number - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- gpuTransform<<>>(in_x, in_y, in_z, trans_x, trans_y, trans_z, points_number, dtrans); +- rubis::sched::yield_gpu("58_gpuTransform"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- } +- +- dtrans.memFree(); +-} +- +-double GNormalDistributionsTransform::computeStepLengthMT(const Eigen::Matrix &x, Eigen::Matrix &step_dir, +- double step_init, double step_max, double step_min, double &score, +- Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, +- float *trans_x, float *trans_y, float *trans_z, int points_num) +-{ +- double phi_0 = -score; +- double d_phi_0 = -(score_gradient.dot(step_dir)); +- +- Eigen::Matrix x_t; +- +- if (d_phi_0 >= 0) { +- if (d_phi_0 == 0) +- return 0; +- else { +- d_phi_0 *= -1; +- step_dir *= -1; +- } +- } +- +- int max_step_iterations = 10; +- int step_iterations = 0; +- +- +- double mu = 1.e-4; +- double nu = 0.9; +- double a_l = 0, a_u = 0; +- +- double f_l = auxilaryFunction_PsiMT(a_l, phi_0, phi_0, d_phi_0, mu); +- double g_l = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu); +- +- double f_u = auxilaryFunction_PsiMT(a_u, phi_0, phi_0, d_phi_0, mu); +- double g_u = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu); +- +- bool interval_converged = (step_max - step_min) > 0, open_interval = true; +- +- double a_t = step_init; +- a_t = std::min(a_t, step_max); +- a_t = std::max(a_t, step_min); +- +- x_t = x + step_dir * a_t; +- +- Eigen::Translation translation(static_cast(x_t(0)), static_cast(x_t(1)), static_cast(x_t(2))); +- Eigen::AngleAxis tmp1(static_cast(x_t(3)), Eigen::Vector3f::UnitX()); +- Eigen::AngleAxis tmp2(static_cast(x_t(4)), Eigen::Vector3f::UnitY()); +- Eigen::AngleAxis tmp3(static_cast(x_t(5)), Eigen::Vector3f::UnitZ()); +- Eigen::AngleAxis tmp4(tmp1 * tmp2 * tmp3); +- +- final_transformation_ = (translation * tmp4).matrix(); +- +- transformPointCloud(x_, y_, z_, trans_x, trans_y, trans_z, points_num, final_transformation_); +- +- score = computeDerivatives(score_gradient, hessian, trans_x, trans_y, trans_z, points_num, x_t); +- +- double phi_t = -score; +- double d_phi_t = -(score_gradient.dot(step_dir)); +- double psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu); +- double d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu); +- +- while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 && d_phi_t <= -nu * d_phi_0)) { +- if (open_interval) { +- a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t); +- } else { +- a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t); +- } +- +- a_t = (a_t < step_max) ? a_t : step_max; +- a_t = (a_t > step_min) ? a_t : step_min; +- +- x_t = x + step_dir * a_t; +- +- translation = Eigen::Translation(static_cast(x_t(0)), static_cast(x_t(1)), static_cast(x_t(2))); +- tmp1 = Eigen::AngleAxis(static_cast(x_t(3)), Eigen::Vector3f::UnitX()); +- tmp2 = Eigen::AngleAxis(static_cast(x_t(4)), Eigen::Vector3f::UnitY()); +- tmp3 = Eigen::AngleAxis(static_cast(x_t(5)), Eigen::Vector3f::UnitZ()); +- tmp4 = tmp1 * tmp2 * tmp3; +- +- final_transformation_ = (translation * tmp4).matrix(); +- +- transformPointCloud(x_, y_, z_, trans_x, trans_y, trans_z, points_num, final_transformation_); +- +- score = computeDerivatives(score_gradient, hessian, trans_x, trans_y, trans_z, points_num, x_t, false); +- +- phi_t -= score; +- d_phi_t -= (score_gradient.dot(step_dir)); +- psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu); +- d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu); +- +- if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) { +- open_interval = false; +- +- f_l += phi_0 - mu * d_phi_0 * a_l; +- g_l += mu * d_phi_0; +- +- f_u += phi_0 - mu * d_phi_0 * a_u; +- g_u += mu * d_phi_0; +- } +- +- if (open_interval) { +- interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t); +- } else { +- interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t); +- } +- step_iterations++; +- } +- +- if (step_iterations) { +- computeHessian(hessian, trans_x, trans_y, trans_z, points_num, x_t); +- } +- +- real_iterations_ += step_iterations; +- +- return a_t; +-} +- +- +-//Copied from ndt.hpp +-double GNormalDistributionsTransform::trialValueSelectionMT (double a_l, double f_l, double g_l, +- double a_u, double f_u, double g_u, +- double a_t, double f_t, double g_t) +-{ +- // Case 1 in Trial Value Selection [More, Thuente 1994] +- if (f_t > f_l) { +- // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t +- // Equation 2.4.52 [Sun, Yuan 2006] +- double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; +- double w = std::sqrt (z * z - g_t * g_l); +- // Equation 2.4.56 [Sun, Yuan 2006] +- double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); +- +- // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l +- // Equation 2.4.2 [Sun, Yuan 2006] +- double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); +- +- if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l)) +- return (a_c); +- else +- return (0.5 * (a_q + a_c)); +- } +- // Case 2 in Trial Value Selection [More, Thuente 1994] +- else if (g_t * g_l < 0) { +- // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t +- // Equation 2.4.52 [Sun, Yuan 2006] +- double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; +- double w = std::sqrt (z * z - g_t * g_l); +- // Equation 2.4.56 [Sun, Yuan 2006] +- double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); +- +- // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t +- // Equation 2.4.5 [Sun, Yuan 2006] +- double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; +- +- if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t)) +- return (a_c); +- else +- return (a_s); +- } +- // Case 3 in Trial Value Selection [More, Thuente 1994] +- else if (std::fabs (g_t) <= std::fabs (g_l)) { +- // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t +- // Equation 2.4.52 [Sun, Yuan 2006] +- double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; +- double w = std::sqrt (z * z - g_t * g_l); +- double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); +- +- // Calculate the minimizer of the quadratic that interpolates g_l and g_t +- // Equation 2.4.5 [Sun, Yuan 2006] +- double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; +- +- double a_t_next; +- +- if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t)) +- a_t_next = a_c; +- else +- a_t_next = a_s; +- +- if (a_t > a_l) +- return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next)); +- else +- return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next)); +- } +- // Case 4 in Trial Value Selection [More, Thuente 1994] +- else { +- // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t +- // Equation 2.4.52 [Sun, Yuan 2006] +- double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; +- double w = std::sqrt (z * z - g_t * g_u); +- // Equation 2.4.56 [Sun, Yuan 2006] +- return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); +- } +-} +- +-//Copied from ndt.hpp +-double GNormalDistributionsTransform::updateIntervalMT (double &a_l, double &f_l, double &g_l, +- double &a_u, double &f_u, double &g_u, +- double a_t, double f_t, double g_t) +-{ +- // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] +- if (f_t > f_l) { +- a_u = a_t; +- f_u = f_t; +- g_u = g_t; +- return (false); +- } +- // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] +- else if (g_t * (a_l - a_t) > 0) { +- a_l = a_t; +- f_l = f_t; +- g_l = g_t; +- return (false); +- } +- // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] +- else if (g_t * (a_l - a_t) < 0) { +- a_u = a_l; +- f_u = f_l; +- g_u = g_l; +- +- a_l = a_t; +- f_l = f_t; +- g_l = g_t; +- return (false); +- } +- // Interval Converged +- else +- return (true); +-} +- +-void GNormalDistributionsTransform::computeHessian(Eigen::Matrix &hessian, float *trans_x, float *trans_y, float *trans_z, int points_num, Eigen::Matrix &p) +-{ +- int *valid_points, *voxel_id, *starting_voxel_id; +- int valid_voxel_num, valid_points_num; +- //Radius Search +- voxel_grid_.radiusSearch(trans_x, trans_y, trans_z, points_num, resolution_, INT_MAX, &valid_points, &starting_voxel_id, &voxel_id, &valid_voxel_num, &valid_points_num); +- +- double *centroid = voxel_grid_.getCentroidList(); +- double *covariance = voxel_grid_.getCovarianceList(); +- double *inverse_covariance = voxel_grid_.getInverseCovarianceList(); +- int *points_per_voxel = voxel_grid_.getPointsPerVoxelList(); +- int voxel_num = voxel_grid_.getVoxelNum(); +- +- if (valid_points_num <= 0) +- return; +- +- //Update score gradient and hessian matrix +- double *hessians, *point_gradients, *point_hessians; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&hessians, sizeof(double) * valid_points_num * 6 * 6)); +- rubis::sched::yield_gpu("59_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&point_gradients, sizeof(double) * valid_points_num * 3 * 6)); +- rubis::sched::yield_gpu("60_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&point_hessians, sizeof(double) * valid_points_num * 18 * 6)); +- rubis::sched::yield_gpu("61_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(hessians, 0, sizeof(double) * valid_points_num * 6 * 6)); +- rubis::sched::yield_gpu("62_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(point_gradients, 0, sizeof(double) * valid_points_num * 3 * 6)); +- rubis::sched::yield_gpu("63_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(point_hessians, 0, sizeof(double) * valid_points_num * 18 * 6)); +- rubis::sched::yield_gpu("64_cudaMemset"); +- +- int block_x = (valid_points_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : valid_points_num; +- int grid_x = (valid_points_num - 1) / block_x + 1; +- dim3 grid; +- +- rubis::sched::request_gpu(); +- computePointGradients0<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dj_ang_.buffer(), +- point_gradients, +- point_gradients + valid_points_num * 7, +- point_gradients + valid_points_num * 14, +- point_gradients + valid_points_num * 9, +- point_gradients + valid_points_num * 15, +- point_gradients + valid_points_num * 4, +- point_gradients + valid_points_num * 10); +- rubis::sched::yield_gpu("65_computePointGradients0"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computePointGradients1<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dj_ang_.buffer(), +- point_gradients + valid_points_num * 16, +- point_gradients + valid_points_num * 5, +- point_gradients + valid_points_num * 11, +- point_gradients + valid_points_num * 17); +- rubis::sched::yield_gpu("66_computePointGradients1"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computePointHessian0<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dh_ang_.buffer(), +- point_hessians + valid_points_num * 57, point_hessians + valid_points_num * 63, point_hessians + valid_points_num * 69, +- point_hessians + valid_points_num * 75, point_hessians + valid_points_num * 58, point_hessians + valid_points_num * 81, +- point_hessians + valid_points_num * 64, point_hessians + valid_points_num * 87, point_hessians + valid_points_num * 70, +- point_hessians + valid_points_num * 93, point_hessians + valid_points_num * 59, point_hessians + valid_points_num * 99, +- point_hessians + valid_points_num * 65, point_hessians + valid_points_num * 105, point_hessians + valid_points_num * 71); +- rubis::sched::yield_gpu("67_computePointHessian0"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computePointHessian1<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dh_ang_.buffer(), +- point_hessians + valid_points_num * 76, point_hessians + valid_points_num * 82, point_hessians + valid_points_num * 88, +- point_hessians + valid_points_num * 94, point_hessians + valid_points_num * 77, point_hessians + valid_points_num * 100, +- point_hessians + valid_points_num * 83, point_hessians + valid_points_num * 106, point_hessians + valid_points_num * 89); +- rubis::sched::yield_gpu("68_computePointHessian1"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computePointHessian2<<>>(x_, y_, z_, points_number_, +- valid_points, valid_points_num, +- dh_ang_.buffer(), +- point_hessians + valid_points_num * 95, point_hessians + valid_points_num * 101, point_hessians + valid_points_num * 107); +- rubis::sched::yield_gpu("69_computePointHessian2"); +- checkCudaErrors(cudaGetLastError()); +- +- double *tmp_hessian; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&tmp_hessian, sizeof(double) * valid_voxel_num * 6)); +- rubis::sched::yield_gpu("70_cudaMalloc"); +- +- double *e_x_cov_x; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&e_x_cov_x, sizeof(double) * valid_voxel_num)); +- rubis::sched::yield_gpu("71_cudaMalloc"); +- +- double *cov_dxd_pi; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&cov_dxd_pi, sizeof(double) * valid_voxel_num * 3 * 6)); +- rubis::sched::yield_gpu("72_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- computeExCovX<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- gauss_d1_, gauss_d2_, +- e_x_cov_x, +- inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num, +- inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num, +- inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num); +- rubis::sched::yield_gpu("73_computeExCovX"); +- checkCudaErrors(cudaGetLastError()); +- +- grid.x = grid_x; +- grid.y = 3; +- grid.z = 6; +- +- rubis::sched::request_gpu(); +- computeCovDxdPi<<>>(valid_points, starting_voxel_id, voxel_id, valid_points_num, +- inverse_covariance, voxel_num, +- gauss_d1_, gauss_d2_, point_gradients, +- cov_dxd_pi, valid_voxel_num); +- rubis::sched::yield_gpu("74_computeCovDxdPi"); +- checkCudaErrors(cudaGetLastError()); +- +- int block_x2 = (valid_voxel_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : valid_voxel_num; +- int grid_x2 = (valid_voxel_num - 1) / block_x2 + 1; +- +- rubis::sched::request_gpu(); +- updateExCovX<<>>(e_x_cov_x, gauss_d2_, valid_voxel_num); +- rubis::sched::yield_gpu("75_updateExCovX"); +- checkCudaErrors(cudaGetLastError()); +- +- grid.y = 6; +- grid.z = 1; +- +- rubis::sched::request_gpu(); +- computeHessianListS0<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num, +- inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num, +- inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num, +- point_gradients, +- tmp_hessian, valid_voxel_num); +- +- rubis::sched::yield_gpu("76_computeHessianListS0"); +- checkCudaErrors(cudaGetLastError()); +- +- grid.z = 6; +- +- rubis::sched::request_gpu(); +- computeHessianListS1<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- gauss_d1_, gauss_d2_, hessians, +- e_x_cov_x, tmp_hessian, cov_dxd_pi, +- point_gradients, +- valid_voxel_num); +- rubis::sched::yield_gpu("77_computeHessianListS1"); +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeHessianListS2<<>>(trans_x, trans_y, trans_z, valid_points, +- starting_voxel_id, voxel_id, valid_points_num, +- centroid, centroid + voxel_num, centroid + 2 * voxel_num, +- gauss_d1_, e_x_cov_x, +- inverse_covariance, inverse_covariance + voxel_num, inverse_covariance + 2 * voxel_num, +- inverse_covariance + 3 * voxel_num, inverse_covariance + 4 * voxel_num, inverse_covariance + 5 * voxel_num, +- inverse_covariance + 6 * voxel_num, inverse_covariance + 7 * voxel_num, inverse_covariance + 8 * voxel_num, +- point_hessians, hessians, valid_voxel_num); +- rubis::sched::yield_gpu("78_computeHessianListS2"); +- +- checkCudaErrors(cudaGetLastError()); +- +- +- int full_size = valid_points_num; +- int half_size = (full_size - 1) / 2 + 1; +- +- while (full_size > 1) { +- block_x = (half_size > BLOCK_SIZE_X) ? BLOCK_SIZE_X : half_size; +- grid_x = (half_size - 1) / block_x + 1; +- +- grid.x = grid_x; +- grid.y = 6; +- grid.z = 6; +- matrixSum<<>>(hessians, full_size, half_size, 6, 6, valid_points_num); +- +- full_size = half_size; +- half_size = (full_size - 1) / 2 + 1; +- } +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- MatrixDevice dhessian(6, 6, valid_points_num, hessians); +- MatrixHost hhessian(6, 6); +- +- hhessian.moveToHost(dhessian); +- +- for (int i = 0; i < 6; i++) { +- for (int j = 0; j < 6; j++) { +- hessian(i, j) = hhessian(i, j); +- } +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(hessians)); +- rubis::sched::yield_gpu("79_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(point_hessians)); +- rubis::sched::yield_gpu("80_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(point_gradients)); +- rubis::sched::yield_gpu("81_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(tmp_hessian)); +- rubis::sched::yield_gpu("82_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(e_x_cov_x)); +- rubis::sched::yield_gpu("83_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(cov_dxd_pi)); +- rubis::sched::yield_gpu("84_free"); +- +- if (valid_points != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_points)); +- rubis::sched::yield_gpu("85_free"); +- } +- +- if (voxel_id != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(voxel_id)); +- rubis::sched::yield_gpu("86_free"); +- } +- +- if (starting_voxel_id != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(starting_voxel_id)); +- rubis::sched::yield_gpu("87_free"); +- } +- +- dhessian.memFree(); +-} +- +-template +-__global__ void gpuSum(T *input, int size, int half_size) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = idx; i < half_size; i += stride) { +- if (i + half_size < size) { +- input[i] += (half_size < size) ? input[i + half_size] : 0; +- } +- } +-} +- +-double GNormalDistributionsTransform::getFitnessScore(double max_range) +-{ +- double fitness_score = 0.0; +- +- float *trans_x, *trans_y, *trans_z; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_x, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("88_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_y, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("89_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_z, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("90_cudaMalloc"); +- +- transformPointCloud(x_, y_, z_, trans_x, trans_y, trans_z, points_number_, final_transformation_); +- +- int *valid_distance; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&valid_distance, sizeof(int) * points_number_)); +- rubis::sched::yield_gpu("91_cudaMalloc"); +- +- double *min_distance; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&min_distance, sizeof(double) * points_number_)); +- rubis::sched::yield_gpu("92_cudaMalloc"); +- +- voxel_grid_.nearestNeighborSearch(trans_x, trans_y, trans_z, points_number_, valid_distance, min_distance, max_range); +- +- int size = points_number_; +- int half_size; +- +- while (size > 1) { +- half_size = (size - 1) / 2 + 1; +- +- int block_x = (half_size > BLOCK_SIZE_X) ? BLOCK_SIZE_X : half_size; +- int grid_x = (half_size - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- gpuSum<<>>(min_distance, size, half_size); +- rubis::sched::yield_gpu("93_gpuSum"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- gpuSum<<>>(valid_distance, size, half_size); +- rubis::sched::yield_gpu("94_gpuSum"); +- +- checkCudaErrors(cudaGetLastError()); +- +- size = half_size; +- } +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- int nr; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&nr, valid_distance, sizeof(int), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("95_dtoh"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&fitness_score, min_distance, sizeof(double), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("96_dtoh"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_x)); +- rubis::sched::yield_gpu("97_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_y)); +- rubis::sched::yield_gpu("98_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_z)); +- rubis::sched::yield_gpu("99_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_distance)); +- rubis::sched::yield_gpu("100_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_distance)); +- rubis::sched::yield_gpu("101_free"); +- +- if (nr > 0) +- return (fitness_score / nr); +- +- return DBL_MAX; +-} +-} +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/Registration.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/Registration.cu +deleted file mode 100644 +index d7962776..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/Registration.cu ++++ /dev/null +@@ -1,624 +0,0 @@ +-#include "ndt_gpu/Registration.h" +-#include "ndt_gpu/debug.h" +-#include +-#include +-#include "rubis_lib/sched.hpp" +- +-//using namespace rubis::sched; +- +-namespace gpu { +-GRegistration::GRegistration() +-{ +- max_iterations_ = 0; +- x_ = y_ = z_ = NULL; +- points_number_ = 0; +- +- trans_x_ = trans_y_ = trans_z_ = NULL; +- +- converged_ = false; +- nr_iterations_ = 0; +- +- transformation_epsilon_ = 0; +- target_cloud_updated_ = true; +- target_points_number_ = 0; +- +- target_x_ = target_y_ = target_z_ = NULL; +- is_copied_ = false; +-} +- +-GRegistration::GRegistration(const GRegistration &other) +-{ +- transformation_epsilon_ = other.transformation_epsilon_; +- max_iterations_ = other.max_iterations_; +- +- //Original scanned point clouds +- x_ = other.x_; +- y_ = other.y_; +- z_ = other.z_; +- +- points_number_ = other.points_number_; +- +- trans_x_ = other.trans_x_; +- trans_y_ = other.trans_y_; +- trans_z_ = other.trans_z_; +- +- converged_ = other.converged_; +- +- nr_iterations_ = other.nr_iterations_; +- final_transformation_ = other.final_transformation_; +- transformation_ = other.transformation_; +- previous_transformation_ = other.previous_transformation_; +- +- target_cloud_updated_ = other.target_cloud_updated_; +- +- target_x_ = other.target_x_; +- target_y_ = other.target_y_; +- target_z_ = other.target_z_; +- +- target_points_number_ = other.target_points_number_; +- is_copied_ = true; +-} +- +-GRegistration::~GRegistration() +-{ +- if (!is_copied_) { +- if (x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(x_)); +- rubis::sched::yield_gpu("102_free"); +- x_ = NULL; +- } +- +- if (y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(y_)); +- rubis::sched::yield_gpu("103_free"); +- +- y_ = NULL; +- } +- +- if (z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(z_)); +- rubis::sched::yield_gpu("104_free"); +- +- z_ = NULL; +- } +- +- if (trans_x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_x_)); +- rubis::sched::yield_gpu("105_free"); +- +- trans_x_ = NULL; +- } +- +- if (trans_y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_y_)); +- rubis::sched::yield_gpu("106_free"); +- +- trans_y_ = NULL; +- } +- +- if (trans_z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_z_)); +- rubis::sched::yield_gpu("107_free"); +- +- trans_z_ = NULL; +- } +- +- if (target_x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_x_)); +- rubis::sched::yield_gpu("108_free"); +- +- target_x_ = NULL; +- } +- +- if (target_y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_y_)); +- rubis::sched::yield_gpu("109_free"); +- +- target_y_ = NULL; +- } +- +- if (target_z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_z_)); +- rubis::sched::yield_gpu("110_free"); +- +- target_z_ = NULL; +- } +- } +-} +- +-void GRegistration::setTransformationEpsilon(double trans_eps) +-{ +- transformation_epsilon_ = trans_eps; +-} +- +-double GRegistration::getTransformationEpsilon() const +-{ +- return transformation_epsilon_; +-} +- +-void GRegistration::setMaximumIterations(int max_itr) +-{ +- max_iterations_ = max_itr; +-} +- +-int GRegistration::getMaximumIterations() const +-{ +- return max_iterations_; +-} +- +-Eigen::Matrix GRegistration::getFinalTransformation() const +-{ +- return final_transformation_; +-} +- +-int GRegistration::getFinalNumIteration() const +-{ +- return nr_iterations_; +-} +- +-bool GRegistration::hasConverged() const +-{ +- return converged_; +-} +- +- +-template +-__global__ void convertInput(T *input, float *out_x, float *out_y, float *out_z, int point_num) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = idx; i < point_num; i += stride) { +- T tmp = input[i]; +- out_x[i] = tmp.x; +- out_y[i] = tmp.y; +- out_z[i] = tmp.z; +- } +-} +- +-void GRegistration::setInputSource(pcl::PointCloud::Ptr input) +-{ +- //Convert point cloud to float x, y, z +- if (input->size() > 0) { +- points_number_ = input->size(); +- +- pcl::PointXYZI *tmp; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&tmp, sizeof(pcl::PointXYZI) * points_number_)); +- rubis::sched::yield_gpu("111_cudaMalloc"); +- +- pcl::PointXYZI *host_tmp = input->points.data(); +- +- // Pin the host buffer for accelerating the memory copy +-#ifndef __aarch64__ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaHostRegister(host_tmp, sizeof(pcl::PointXYZI) * points_number_, cudaHostRegisterDefault)); +- rubis::sched::yield_gpu("112_htod"); +-#endif +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(tmp, host_tmp, sizeof(pcl::PointXYZI) * points_number_, cudaMemcpyHostToDevice)); +- rubis::sched::yield_gpu("113_htod"); +- +- +- if (x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(x_)); +- rubis::sched::yield_gpu("114_free"); +- x_ = NULL; +- } +- +- if (y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(y_)); +- rubis::sched::yield_gpu("115_free"); +- y_ = NULL; +- } +- +- if (z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(z_)); +- rubis::sched::yield_gpu("116_free"); +- z_ = NULL; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&x_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("117_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&y_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("118_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&z_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("119_cudaMalloc"); +- +- int block_x = (points_number_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : points_number_; +- int grid_x = (points_number_ - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- convertInput<<>>(tmp, x_, y_, z_, points_number_); +- rubis::sched::yield_gpu("120_convertInput"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- +- if (trans_x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_x_)); +- rubis::sched::yield_gpu("121_free"); +- trans_x_ = NULL; +- } +- +- if (trans_y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_y_)); +- rubis::sched::yield_gpu("122_free"); +- trans_y_ = NULL; +- } +- +- if (trans_z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_z_)); +- rubis::sched::yield_gpu("123_free"); +- trans_z_ = NULL; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_x_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("124_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_y_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("125_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_z_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("126_cudaMalloc"); +- +- // Initially, also copy scanned points to transformed buffers +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(trans_x_, x_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("127_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(trans_y_, y_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("128_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(trans_z_, z_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("129_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(tmp)); +- rubis::sched::yield_gpu("130_free"); +- +- // Unpin host buffer +-#ifndef __aarch64__ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaHostUnregister(host_tmp)); +- rubis::sched::yield_gpu("131_free"); +-#endif +- } +-} +- +-void GRegistration::setInputSource(pcl::PointCloud::Ptr input) +-{ +- //Convert point cloud to float x, y, z +- if (input->size() > 0) { +- points_number_ = input->size(); +- +- pcl::PointXYZ *tmp; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&tmp, sizeof(pcl::PointXYZ) * points_number_)); +- rubis::sched::yield_gpu("132_cudaMalloc"); +- +- pcl::PointXYZ *host_tmp = input->points.data(); +- +- // Pin the host buffer for accelerating the memory copy +-#ifndef __aarch64__ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaHostRegister(host_tmp, sizeof(pcl::PointXYZ) * points_number_, cudaHostRegisterDefault)); +- rubis::sched::yield_gpu("133_htod"); +-#endif +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(tmp, host_tmp, sizeof(pcl::PointXYZ) * points_number_, cudaMemcpyHostToDevice)); +- rubis::sched::yield_gpu("134_htod"); +- +- if (x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(x_)); +- rubis::sched::yield_gpu("135_free"); +- +- x_ = NULL; +- } +- +- if (y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(y_)); +- rubis::sched::yield_gpu("136_free"); +- +- y_ = NULL; +- } +- +- if (z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(z_)); +- rubis::sched::yield_gpu("137_free"); +- +- z_ = NULL; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&x_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("138_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&y_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("139_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&z_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("140_cudaMalloc"); +- +- int block_x = (points_number_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : points_number_; +- int grid_x = (points_number_ - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- convertInput<<>>(tmp, x_, y_, z_, points_number_); +- rubis::sched::yield_gpu("141_convertInput"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- if (trans_x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_x_)); +- rubis::sched::yield_gpu("142_free"); +- +- trans_x_ = NULL; +- } +- +- if (trans_y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_y_)); +- rubis::sched::yield_gpu("143_free"); +- +- trans_y_ = NULL; +- } +- +- if (trans_z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(trans_z_)); +- rubis::sched::yield_gpu("144_free"); +- +- trans_z_ = NULL; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_x_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("145_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_y_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("146_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&trans_z_, sizeof(float) * points_number_)); +- rubis::sched::yield_gpu("147_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(trans_x_, x_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("148_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(trans_y_, y_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("149_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(trans_z_, z_, sizeof(float) * points_number_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("150_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(tmp)); +- rubis::sched::yield_gpu("151_free"); +- +-#ifndef __aarch64__ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaHostUnregister(host_tmp)); +- rubis::sched::yield_gpu("152_free"); +-#endif +- } +-} +- +- +- +-//Set input MAP data +-void GRegistration::setInputTarget(pcl::PointCloud::Ptr input) +-{ +- if (input->size() > 0) { +- target_points_number_ = input->size(); +- +- pcl::PointXYZI *tmp; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&tmp, sizeof(pcl::PointXYZI) * target_points_number_)); +- rubis::sched::yield_gpu("153_cudaMalloc"); +- +- pcl::PointXYZI *host_tmp = input->points.data(); +- +-#ifndef __aarch64__ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaHostRegister(host_tmp, sizeof(pcl::PointXYZI) * target_points_number_, cudaHostRegisterDefault)); +- rubis::sched::yield_gpu("154_htod"); +-#endif +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(tmp, host_tmp, sizeof(pcl::PointXYZI) * target_points_number_, cudaMemcpyHostToDevice)); +- rubis::sched::yield_gpu("155_htod"); +- +- if (target_x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_x_)); +- rubis::sched::yield_gpu("156_free"); +- +- target_x_ = NULL; +- } +- +- if (target_y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_y_)); +- rubis::sched::yield_gpu("157_free"); +- +- target_y_ = NULL; +- } +- +- if (target_z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_z_)); +- rubis::sched::yield_gpu("158_free"); +- +- target_z_ = NULL; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&target_x_, sizeof(float) * target_points_number_)); +- rubis::sched::yield_gpu("159_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&target_y_, sizeof(float) * target_points_number_)); +- rubis::sched::yield_gpu("160_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&target_z_, sizeof(float) * target_points_number_)); +- rubis::sched::yield_gpu("161_cudaMalloc"); +- +- int block_x = (target_points_number_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : target_points_number_; +- int grid_x = (target_points_number_ - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- convertInput<<>>(tmp, target_x_, target_y_, target_z_, target_points_number_); +- rubis::sched::yield_gpu("162_convertInput"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +-#ifndef __aarch64__ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaHostUnregister(host_tmp)); +- rubis::sched::yield_gpu("163_free"); +-#endif +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(tmp)); +- rubis::sched::yield_gpu("164_free"); +- } +-} +- +-void GRegistration::setInputTarget(pcl::PointCloud::Ptr input) +-{ +- if (input->size() > 0) { +- target_points_number_ = input->size(); +- +- pcl::PointXYZ *tmp; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&tmp, sizeof(pcl::PointXYZ) * target_points_number_)); +- rubis::sched::yield_gpu("165_cudaMalloc"); +- +- pcl::PointXYZ *host_tmp = input->points.data(); +- +-#ifndef __aarch64__ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaHostRegister(host_tmp, sizeof(pcl::PointXYZ) * target_points_number_, cudaHostRegisterDefault)); +- rubis::sched::yield_gpu("166_htod"); +-#endif +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(tmp, host_tmp, sizeof(pcl::PointXYZ) * target_points_number_, cudaMemcpyHostToDevice)); +- rubis::sched::yield_gpu("167_htod"); +- +- if (target_x_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_x_)); +- rubis::sched::yield_gpu("168_free"); +- +- target_x_ = NULL; +- } +- +- if (target_y_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_y_)); +- rubis::sched::yield_gpu("169_free"); +- +- target_y_ = NULL; +- } +- +- if (target_z_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(target_z_)); +- rubis::sched::yield_gpu("170_free"); +- +- target_z_ = NULL; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&target_x_, sizeof(float) * target_points_number_)); +- rubis::sched::yield_gpu("171_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&target_y_, sizeof(float) * target_points_number_)); +- rubis::sched::yield_gpu("172_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&target_z_, sizeof(float) * target_points_number_)); +- rubis::sched::yield_gpu("173_cudaMalloc"); +- +- int block_x = (target_points_number_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : target_points_number_; +- int grid_x = (target_points_number_ - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- convertInput<<>>(tmp, target_x_, target_y_, target_z_, target_points_number_); +- rubis::sched::yield_gpu("174_convertInput"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(tmp)); +- rubis::sched::yield_gpu("175_free"); +-#ifndef __aarch64__ +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaHostUnregister(host_tmp)); +- rubis::sched::yield_gpu("176_free"); +-#endif +- } +-} +- +-void GRegistration::align(const Eigen::Matrix &guess) +-{ +- converged_ = false; +- +- final_transformation_ = transformation_ = previous_transformation_ = Eigen::Matrix::Identity(); +- +- computeTransformation(guess); +-} +- +-void GRegistration::computeTransformation(const Eigen::Matrix &guess) { +- printf("Unsupported by Registration\n"); +-} +-} +- +- +- +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/SymmetricEigenSolver.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/SymmetricEigenSolver.cu +deleted file mode 100644 +index cad83da7..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/SymmetricEigenSolver.cu ++++ /dev/null +@@ -1,90 +0,0 @@ +-#include "ndt_gpu/SymmetricEigenSolver.h" +-#include "ndt_gpu/debug.h" +-#include "rubis_lib/sched.hpp" +- +-namespace gpu { +- +-SymmetricEigensolver3x3::SymmetricEigensolver3x3(int offset) +-{ +- offset_ = offset; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&buffer_, sizeof(double) * 18 * offset_)); +- rubis::sched::yield_gpu("177_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&maxAbsElement_, sizeof(double) * offset_)); +- rubis::sched::yield_gpu("178_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&norm_, sizeof(double) * offset_)); +- rubis::sched::yield_gpu("179_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&i02_, sizeof(int) * 2 * offset_)); +- rubis::sched::yield_gpu("180_cudaMalloc"); +- +- eigenvectors_ = NULL; +- eigenvalues_ = NULL; +- input_matrices_ = NULL; +- +- is_copied_ = false; +-} +- +-void SymmetricEigensolver3x3::setInputMatrices(double *input_matrices) +-{ +- input_matrices_ = input_matrices; +-} +- +-void SymmetricEigensolver3x3::setEigenvectors(double *eigenvectors) +-{ +- eigenvectors_ = eigenvectors; +-} +- +-void SymmetricEigensolver3x3::setEigenvalues(double *eigenvalues) +-{ +- eigenvalues_ = eigenvalues; +-} +- +-double* SymmetricEigensolver3x3::getBuffer() const +-{ +- return buffer_; +-} +- +-void SymmetricEigensolver3x3::memFree() +-{ +- if (!is_copied_) { +- if (buffer_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(buffer_)); +- rubis::sched::yield_gpu("181_free"); +- +- buffer_ = NULL; +- } +- +- if (maxAbsElement_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(maxAbsElement_)); +- rubis::sched::yield_gpu("182_free"); +- +- maxAbsElement_ = NULL; +- } +- +- if (norm_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(norm_)); +- rubis::sched::yield_gpu("183_free"); +- +- norm_ = NULL; +- } +- +- if (i02_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(i02_)); +- rubis::sched::yield_gpu("184_free"); +- +- i02_ = NULL; +- } +- } +-} +-} +diff --git a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/VoxelGrid.cu b/autoware.ai/src/autoware/core_perception/ndt_gpu/src/VoxelGrid.cu +deleted file mode 100644 +index 5d7def9f..00000000 +--- a/autoware.ai/src/autoware/core_perception/ndt_gpu/src/VoxelGrid.cu ++++ /dev/null +@@ -1,2125 +0,0 @@ +-#include "ndt_gpu/VoxelGrid.h" +-#include "ndt_gpu/debug.h" +-#include "ndt_gpu/common.h" +-#include +-#include +-#include +-#include +-#include +-#include +-#include +-#include +- +-#include +-#include +- +-#include +-#include +-#include "rubis_lib/sched.hpp" +- +-#include "ndt_gpu/SymmetricEigenSolver.h" +- +-namespace gpu { +- +-GVoxelGrid::GVoxelGrid(): +- x_(NULL), +- y_(NULL), +- z_(NULL), +- points_num_(0), +- centroid_(NULL), +- covariance_(NULL), +- inverse_covariance_(NULL), +- points_per_voxel_(NULL), +- voxel_num_(0), +- max_x_(FLT_MAX), +- max_y_(FLT_MAX), +- max_z_(FLT_MAX), +- min_x_(FLT_MIN), +- min_y_(FLT_MIN), +- min_z_(FLT_MIN), +- voxel_x_(0), +- voxel_y_(0), +- voxel_z_(0), +- max_b_x_(0), +- max_b_y_(0), +- max_b_z_(0), +- min_b_x_(0), +- min_b_y_(0), +- min_b_z_(0), +- vgrid_x_(0), +- vgrid_y_(0), +- vgrid_z_(0), +- min_points_per_voxel_(6), +- starting_point_ids_(NULL), +- point_ids_(NULL), +- is_copied_(false) +-{ +- +-}; +- +-GVoxelGrid::GVoxelGrid(const GVoxelGrid &other) +-{ +- x_ = other.x_; +- y_ = other.y_; +- z_ = other.z_; +- +- points_num_ = other.points_num_; +- +- centroid_ = other.centroid_; +- covariance_ = other.covariance_; +- inverse_covariance_ = other.inverse_covariance_; +- points_per_voxel_ = other.points_per_voxel_; +- +- voxel_num_ = other.voxel_num_; +- max_x_ = other.max_x_; +- max_y_ = other.max_y_; +- max_z_ = other.max_z_; +- +- min_x_ = other.min_x_; +- min_y_ = other.min_y_; +- min_z_ = other.min_z_; +- +- voxel_x_ = other.voxel_x_; +- voxel_y_ = other.voxel_y_; +- voxel_z_ = other.voxel_z_; +- +- max_b_x_ = other.max_b_x_; +- max_b_y_ = other.max_b_y_; +- max_b_z_ = other.max_b_z_; +- +- min_b_x_ = other.min_b_x_; +- min_b_y_ = other.min_b_y_; +- min_b_z_ = other.min_b_z_; +- +- vgrid_x_ = other.vgrid_x_; +- vgrid_y_ = other.vgrid_y_; +- vgrid_z_ = other.vgrid_z_; +- +- min_points_per_voxel_ = other.min_points_per_voxel_; +- +- starting_point_ids_ = other.starting_point_ids_; +- point_ids_ = other.point_ids_; +- +- +- is_copied_ = true; +-} +- +- +-GVoxelGrid::~GVoxelGrid() { +- if (!is_copied_) { +- +- for (unsigned int i = 1; i < octree_centroids_.size(); i++) { +- if (octree_centroids_[i] != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(octree_centroids_[i])); +- rubis::sched::yield_gpu("185_free"); +- +- octree_centroids_[i] = NULL; +- } +- if (octree_points_per_node_[i] != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(octree_points_per_node_[i])); +- rubis::sched::yield_gpu("186_free"); +- +- octree_points_per_node_[i] = NULL; +- } +- } +- +- octree_centroids_.clear(); +- octree_points_per_node_.clear(); +- octree_grid_size_.clear(); +- +- if (starting_point_ids_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(starting_point_ids_)); +- rubis::sched::yield_gpu("187_free"); +- +- starting_point_ids_ = NULL; +- } +- +- if (point_ids_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(point_ids_)); +- rubis::sched::yield_gpu("188_free"); +- +- point_ids_ = NULL; +- } +- +- if (centroid_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(centroid_)); +- rubis::sched::yield_gpu("189_free"); +- +- centroid_ = NULL; +- } +- +- if (covariance_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(covariance_)); +- rubis::sched::yield_gpu("190_free"); +- +- covariance_ = NULL; +- } +- +- if (inverse_covariance_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(inverse_covariance_)); +- rubis::sched::yield_gpu("191_free"); +- +- inverse_covariance_ = NULL; +- } +- +- if (points_per_voxel_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(points_per_voxel_)); +- rubis::sched::yield_gpu("192_free"); +- +- points_per_voxel_ = NULL; +- } +- } +-} +- +- +-void GVoxelGrid::initialize() +-{ +- if (centroid_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(centroid_)); +- rubis::sched::yield_gpu("193_free"); +- +- centroid_ = NULL; +- } +- +- if (covariance_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(covariance_)); +- rubis::sched::yield_gpu("194_free"); +- +- covariance_ = NULL; +- } +- +- if (inverse_covariance_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(inverse_covariance_)); +- rubis::sched::yield_gpu("195_free"); +- +- inverse_covariance_ = NULL; +- } +- +- if (points_per_voxel_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(points_per_voxel_)); +- rubis::sched::yield_gpu("196_free"); +- +- points_per_voxel_ = NULL; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(¢roid_, sizeof(double) * 3 * voxel_num_)); +- rubis::sched::yield_gpu("197_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&covariance_, sizeof(double) * 9 * voxel_num_)); +- rubis::sched::yield_gpu("198_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&inverse_covariance_, sizeof(double) * 9 * voxel_num_)); +- rubis::sched::yield_gpu("199_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&points_per_voxel_, sizeof(int) * voxel_num_)); +- rubis::sched::yield_gpu("200_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(inverse_covariance_, 0, sizeof(double) * 9 * voxel_num_)); +- rubis::sched::yield_gpu("201_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(points_per_voxel_, 0, sizeof(int) * voxel_num_)); +- rubis::sched::yield_gpu("202_cudaMemset"); +- +- checkCudaErrors(cudaDeviceSynchronize()); +-} +- +-int GVoxelGrid::getVoxelNum() const +-{ +- return voxel_num_; +-} +- +- +-float GVoxelGrid::getMaxX() const +-{ +- return max_x_; +-} +-float GVoxelGrid::getMaxY() const +-{ +- return max_y_; +-} +-float GVoxelGrid::getMaxZ() const +-{ +- return max_z_; +-} +- +- +-float GVoxelGrid::getMinX() const +-{ +- return min_x_; +-} +-float GVoxelGrid::getMinY() const +-{ +- return min_y_; +-} +-float GVoxelGrid::getMinZ() const +-{ +- return min_z_; +-} +- +- +-float GVoxelGrid::getVoxelX() const +-{ +- return voxel_x_; +-} +-float GVoxelGrid::getVoxelY() const +-{ +- return voxel_y_; +-} +-float GVoxelGrid::getVoxelZ() const +-{ +- return voxel_z_; +-} +- +- +-int GVoxelGrid::getMaxBX() const +-{ +- return max_b_x_; +-} +-int GVoxelGrid::getMaxBY() const +-{ +- return max_b_y_; +-} +-int GVoxelGrid::getMaxBZ() const +-{ +- return max_b_z_; +-} +- +- +-int GVoxelGrid::getMinBX() const +-{ +- return min_b_x_; +-} +-int GVoxelGrid::getMinBY() const +-{ +- return min_b_y_; +-} +-int GVoxelGrid::getMinBZ() const +-{ +- return min_b_z_; +-} +- +- +-int GVoxelGrid::getVgridX() const +-{ +- return vgrid_x_; +-} +-int GVoxelGrid::getVgridY() const +-{ +- return vgrid_y_; +-} +-int GVoxelGrid::getVgridZ() const +-{ +- return vgrid_z_; +-} +- +- +-void GVoxelGrid::setLeafSize(float voxel_x, float voxel_y, float voxel_z) +-{ +- voxel_x_ = voxel_x; +- voxel_y_ = voxel_y; +- voxel_z_ = voxel_z; +-} +- +-double* GVoxelGrid::getCentroidList() const +-{ +- return centroid_; +-} +- +-double* GVoxelGrid::getCovarianceList() const +-{ +- return covariance_; +-} +- +-double* GVoxelGrid::getInverseCovarianceList() const +-{ +- return inverse_covariance_; +-} +- +-int* GVoxelGrid::getPointsPerVoxelList() const +-{ +- return points_per_voxel_; +-} +- +- +-extern "C" __device__ int voxelId(float x, float y, float z, +- float voxel_x, float voxel_y, float voxel_z, +- int min_b_x, int min_b_y, int min_b_z, +- int vgrid_x, int vgrid_y, int vgrid_z) +-{ +- int id_x = static_cast(floorf(x / voxel_x) - static_cast(min_b_x)); +- int id_y = static_cast(floorf(y / voxel_y) - static_cast(min_b_y)); +- int id_z = static_cast(floorf(z / voxel_z) - static_cast(min_b_z)); +- +- return (id_x + id_y * vgrid_x + id_z * vgrid_x * vgrid_y); +-} +- +-/* First step to compute centroids and covariances of voxels. */ +-extern "C" __global__ void initCentroidAndCovariance(float *x, float *y, float *z, int *starting_point_ids, int *point_ids, +- double *centroids, double *covariances, int voxel_num) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = idx; i < voxel_num; i += stride) { +- MatrixDevice centr(3, 1, voxel_num, centroids + i); +- MatrixDevice cov(3, 3, voxel_num, covariances + i); +- +- double centr0, centr1, centr2; +- double cov00, cov01, cov02, cov11, cov12, cov22; +- +- centr0 = centr1 = centr2 = 0.0; +- cov00 = cov11 = cov22 = 1.0; +- cov01 = cov02 = cov12 = 0.0; +- +- for (int j = starting_point_ids[i]; j < starting_point_ids[i + 1]; j++) { +- int pid = point_ids[j]; +- double t_x = static_cast(x[pid]); +- double t_y = static_cast(y[pid]); +- double t_z = static_cast(z[pid]); +- +- centr0 += t_x; +- centr1 += t_y; +- centr2 += t_z; +- +- cov00 += t_x * t_x; +- cov01 += t_x * t_y; +- cov02 += t_x * t_z; +- cov11 += t_y * t_y; +- cov12 += t_y * t_z; +- cov22 += t_z * t_z; +- } +- +- centr(0) = centr0; +- centr(1) = centr1; +- centr(2) = centr2; +- +- cov(0, 0) = cov00; +- cov(0, 1) = cov01; +- cov(0, 2) = cov02; +- cov(1, 1) = cov11; +- cov(1, 2) = cov12; +- cov(2, 2) = cov22; +- } +-} +- +-/* Update centroids of voxels. */ +-extern "C" __global__ void updateVoxelCentroid(double *centroid, int *points_per_voxel, int voxel_num) +-{ +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int vid = index; vid < voxel_num; vid += stride) { +- MatrixDevice centr(3, 1, voxel_num, centroid + vid); +- double points_num = static_cast(points_per_voxel[vid]); +- +- if (points_num > 0) { +- centr /= points_num; +- } +- } +-} +- +-/* Update covariance of voxels. */ +-extern "C" __global__ void updateVoxelCovariance(double *centroid, double *pt_sum, double *covariance, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int vid = index; vid < voxel_num; vid += stride) { +- MatrixDevice centr(3, 1, voxel_num, centroid + vid); +- MatrixDevice cov(3, 3, voxel_num, covariance + vid); +- MatrixDevice pts(3, 1, voxel_num, pt_sum + vid); +- double points_num = static_cast(points_per_voxel[vid]); +- +- double c0 = centr(0); +- double c1 = centr(1); +- double c2 = centr(2); +- double p0 = pts(0); +- double p1 = pts(1); +- double p2 = pts(2); +- +- points_per_voxel[vid] = (points_num < min_points_per_voxel) ? 0 : points_num; +- +- if (points_num >= min_points_per_voxel) { +- +- double mult = (points_num - 1.0) / points_num; +- +- cov(0, 0) = ((cov(0, 0) - 2.0 * p0 * c0) / points_num + c0 * c0) * mult; +- cov(0, 1) = ((cov(0, 1) - 2.0 * p0 * c1) / points_num + c0 * c1) * mult; +- cov(0, 2) = ((cov(0, 2) - 2.0 * p0 * c2) / points_num + c0 * c2) * mult; +- cov(1, 0) = cov(0, 1); +- cov(1, 1) = ((cov(1, 1) - 2.0 * p1 * c1) / points_num + c1 * c1) * mult; +- cov(1, 2) = ((cov(1, 2) - 2.0 * p1 * c2) / points_num + c1 * c2) * mult; +- cov(2, 0) = cov(0, 2); +- cov(2, 1) = cov(1, 2); +- cov(2, 2) = ((cov(2, 2) - 2.0 * p2 * c2) / points_num + c2 * c2) * mult; +- } +- } +-} +- +-extern "C" __global__ void computeInverseEigenvectors(double *inverse_covariance, int *points_per_voxel, int voxel_num, double *eigenvectors, int min_points_per_voxel) +-{ +- +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int vid = index; vid < voxel_num; vid += stride) { +- if (points_per_voxel[vid] >= min_points_per_voxel) { +- MatrixDevice icov(3, 3, voxel_num, inverse_covariance + vid); +- MatrixDevice eigen_vectors(3, 3, voxel_num, eigenvectors + vid); +- +- eigen_vectors.inverse(icov); +- } +- +- __syncthreads(); +- } +-} +- +-//eigen_vecs = eigen_vecs * eigen_val +- +-extern "C" __global__ void updateCovarianceS0(int *points_per_voxel, int voxel_num, double *eigenvalues, double *eigenvectors, int min_points_per_voxel) +-{ +- +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int vid = index; vid < voxel_num; vid += stride) { +- if (points_per_voxel[vid] >= min_points_per_voxel) { +- MatrixDevice eigen_vectors(3, 3, voxel_num, eigenvectors + vid); +- +- double eig_val0 = eigenvalues[vid]; +- double eig_val1 = eigenvalues[vid + voxel_num]; +- double eig_val2 = eigenvalues[vid + 2 * voxel_num]; +- +- eigen_vectors(0, 0) *= eig_val0; +- eigen_vectors(1, 0) *= eig_val0; +- eigen_vectors(2, 0) *= eig_val0; +- +- eigen_vectors(0, 1) *= eig_val1; +- eigen_vectors(1, 1) *= eig_val1; +- eigen_vectors(2, 1) *= eig_val1; +- +- eigen_vectors(0, 2) *= eig_val2; +- eigen_vectors(1, 2) *= eig_val2; +- eigen_vectors(2, 2) *= eig_val2; +- } +- +- __syncthreads(); +- } +-} +- +-//cov = new eigen_vecs * eigen_vecs transpose +- +-extern "C" __global__ void updateCovarianceS1(double *covariance, double *inverse_covariance, int *points_per_voxel, int voxel_num, double *eigenvectors, int min_points_per_voxel, int col) +-{ +- +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int vid = index; vid < voxel_num; vid += stride) { +- if (points_per_voxel[vid] >= min_points_per_voxel) { +- MatrixDevice cov(3, 3, voxel_num, covariance + vid); +- MatrixDevice icov(3, 3, voxel_num, inverse_covariance + vid); +- MatrixDevice eigen_vectors(3, 3, voxel_num, eigenvectors + vid); +- +- double tmp0 = icov(0, col); +- double tmp1 = icov(1, col); +- double tmp2 = icov(2, col); +- +- cov(0, col) = eigen_vectors(0, 0) * tmp0 + eigen_vectors(0, 1) * tmp1 + eigen_vectors(0, 2) * tmp2; +- cov(1, col) = eigen_vectors(1, 0) * tmp0 + eigen_vectors(1, 1) * tmp1 + eigen_vectors(1, 2) * tmp2; +- cov(2, col) = eigen_vectors(2, 0) * tmp0 + eigen_vectors(2, 1) * tmp1 + eigen_vectors(2, 2) * tmp2; +- } +- +- __syncthreads(); +- } +-} +- +-extern "C" __global__ void computeInverseCovariance(double *covariance, double *inverse_covariance, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int vid = index; vid < voxel_num; vid += stride) { +- if (points_per_voxel[vid] >= min_points_per_voxel) { +- MatrixDevice cov(3, 3, voxel_num, covariance + vid); +- MatrixDevice icov(3, 3, voxel_num, inverse_covariance + vid); +- +- cov.inverse(icov); +- } +- __syncthreads(); +- } +-} +- +- +-template +-__global__ void init(T *input, int size, int local_size) +-{ +- for (int i = threadIdx.x + blockIdx.x * blockDim.x; i < size; i += blockDim.x * gridDim.x) { +- for (int j = 0; j < local_size; j++) +- input[i + j * size] = 1; +- } +-} +- +-extern "C" __global__ void initBoolean(bool *input, int size) +-{ +- for (int i = threadIdx.x + blockIdx.x * blockDim.x; i < size; i += blockDim.x * gridDim.x) { +- input[i] = (i % 2 == 0) ? true : false; +- } +-} +- +-/* Normalize input matrices to avoid overflow. */ +-extern "C" __global__ void normalize(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) {} +- sv.normalizeInput(id); +- __syncthreads(); +- } +-} +- +-/* Compute eigenvalues. Eigenvalues are arranged in increasing order. +- * (eigen(0) <= eigen(1) <= eigen(2). */ +-extern "C" __global__ void computeEigenvalues(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) +- sv.computeEigenvalues(id); +- __syncthreads(); +- } +-} +- +-/* First step to compute eigenvector 0 of covariance matrices. */ +-extern "C" __global__ void computeEvec00(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) +- sv.computeEigenvector00(id); +- __syncthreads(); +- } +-} +- +-/* Second step to compute eigenvector 0 of covariance matrices. */ +-extern "C" __global__ void computeEvec01(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) +- sv.computeEigenvector01(id); +- __syncthreads(); +- } +-} +- +-/* First step to compute eigenvector 1 of covariance matrices. */ +-extern "C" __global__ void computeEvec10(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) +- sv.computeEigenvector10(id); +- __syncthreads(); +- } +-} +- +-/* Second step to compute eigenvector 1 of covariance matrices. */ +-extern "C" __global__ void computeEvec11(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) +- sv.computeEigenvector11(id); +- __syncthreads(); +- } +-} +- +-/* Compute eigenvector 2 of covariance matrices. */ +-extern "C" __global__ void computeEvec2(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) +- sv.computeEigenvector2(id); +- __syncthreads(); +- } +-} +- +-/* Final step to compute eigenvalues. */ +-extern "C" __global__ void updateEval(SymmetricEigensolver3x3 sv, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) +- sv.updateEigenvalues(id); +- __syncthreads(); +- } +-} +- +-/* Update eigenvalues in the case covariance matrix is nearly singular. */ +-extern "C" __global__ void updateEval2(double *eigenvalues, int *points_per_voxel, int voxel_num, int min_points_per_voxel) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < voxel_num; id += blockDim.x * gridDim.x) { +- if (points_per_voxel[id] >= min_points_per_voxel) { +- MatrixDevice eigen_val(3, 1, voxel_num, eigenvalues + id); +- double ev0 = eigen_val(0); +- double ev1 = eigen_val(1); +- double ev2 = eigen_val(2); +- +- +- if (ev0 < 0 || ev1 < 0 || ev2 <= 0) { +- points_per_voxel[id] = 0; +- continue; +- } +- +- double min_cov_eigvalue = ev2 * 0.01; +- +- if (ev0 < min_cov_eigvalue) { +- ev0 = min_cov_eigvalue; +- +- if (ev1 < min_cov_eigvalue) { +- ev1 = min_cov_eigvalue; +- } +- } +- +- eigen_val(0) = ev0; +- eigen_val(1) = ev1; +- eigen_val(2) = ev2; +- +- __syncthreads(); +- } +- } +-} +- +-void GVoxelGrid::computeCentroidAndCovariance() +-{ +- int block_x = (voxel_num_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : voxel_num_; +- int grid_x = (voxel_num_ - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- initCentroidAndCovariance<<>>(x_, y_, z_, starting_point_ids_, point_ids_, centroid_, covariance_, voxel_num_); +- rubis::sched::yield_gpu("203_initCentroidAndCovariance"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- double *pt_sum; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&pt_sum, sizeof(double) * voxel_num_ * 3)); +- rubis::sched::yield_gpu("204_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(pt_sum, centroid_, sizeof(double) * voxel_num_ * 3, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("205_cudaMemcpy"); +- +- rubis::sched::request_gpu(); +- updateVoxelCentroid<<>>(centroid_, points_per_voxel_, voxel_num_); +- rubis::sched::yield_gpu("206_updateVoxelCentroid"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- updateVoxelCovariance<<>>(centroid_, pt_sum, covariance_, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("207_updateVoxelCovariance"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(pt_sum)); +- rubis::sched::yield_gpu("208_free"); +- +- double *eigenvalues_dev, *eigenvectors_dev; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&eigenvalues_dev, sizeof(double) * 3 * voxel_num_)); +- rubis::sched::yield_gpu("209_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&eigenvectors_dev, sizeof(double) * 9 * voxel_num_)); +- rubis::sched::yield_gpu("210_cudaMalloc"); +- +- // Solving eigenvalues and eigenvectors problem by the GPU. +- SymmetricEigensolver3x3 sv(voxel_num_); +- +- sv.setInputMatrices(covariance_); +- sv.setEigenvalues(eigenvalues_dev); +- sv.setEigenvectors(eigenvectors_dev); +- +- rubis::sched::request_gpu(); +- normalize<<>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("211_normalize"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeEigenvalues<<>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("212_computeEigenvalues"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeEvec00<<>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("213_computeEvec00"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeEvec01<<>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("214_computeEvec01"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeEvec10<<>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("215_computeEvec10"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeEvec11<<>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("216_computeEvec11"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeEvec2<<>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("217_computeEvec2"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- updateEval<<>>(sv, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("218_updateEval"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- updateEval2<<>>(eigenvalues_dev, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("219_updateEval2"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- computeInverseEigenvectors<<>>(inverse_covariance_, points_per_voxel_, voxel_num_, eigenvectors_dev, min_points_per_voxel_); +- rubis::sched::yield_gpu("220_computeInverseEigenvectors"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- updateCovarianceS0<<>>(points_per_voxel_, voxel_num_, eigenvalues_dev, eigenvectors_dev, min_points_per_voxel_); +- rubis::sched::yield_gpu("221_updateCovarianceS0"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- updateCovarianceS1<<>>(covariance_, inverse_covariance_, points_per_voxel_, voxel_num_, eigenvectors_dev, min_points_per_voxel_, 0); +- rubis::sched::yield_gpu("222_updateCovarianceS1"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- updateCovarianceS1<<>>(covariance_, inverse_covariance_, points_per_voxel_, voxel_num_, eigenvectors_dev, min_points_per_voxel_, 1); +- rubis::sched::yield_gpu("223_updateCovarianceS1"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- updateCovarianceS1<<>>(covariance_, inverse_covariance_, points_per_voxel_, voxel_num_, eigenvectors_dev, min_points_per_voxel_, 2); +- rubis::sched::yield_gpu("224_updateCovarianceS1"); +- +- checkCudaErrors(cudaGetLastError()); +- +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- computeInverseCovariance<<>>(covariance_, inverse_covariance_, points_per_voxel_, voxel_num_, min_points_per_voxel_); +- rubis::sched::yield_gpu("225_computeInverseCovariance"); +- +- checkCudaErrors(cudaGetLastError()); +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- sv.memFree(); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(eigenvalues_dev)); +- rubis::sched::yield_gpu("226_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(eigenvectors_dev)); +- rubis::sched::yield_gpu("227_free"); +-} +- +-//Input are supposed to be in device memory +-void GVoxelGrid::setInput(float *x, float *y, float *z, int points_num) +-{ +- if (points_num <= 0) +- return; +- x_ = x; +- y_ = y; +- z_ = z; +- points_num_ = points_num; +- +- findBoundaries(); +- +- voxel_num_ = vgrid_x_ * vgrid_y_ * vgrid_z_; +- +- initialize(); +- +- scatterPointsToVoxelGrid(); +- +- computeCentroidAndCovariance(); +- +- buildOctree(); +-} +- +-/* Find the largest coordinate values */ +-extern "C" __global__ void findMax(float *x, float *y, float *z, int full_size, int half_size) +-{ +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = index; i < half_size; i += stride) { +- x[i] = (i + half_size < full_size) ? ((x[i] >= x[i + half_size]) ? x[i] : x[i + half_size]) : x[i]; +- y[i] = (i + half_size < full_size) ? ((y[i] >= y[i + half_size]) ? y[i] : y[i + half_size]) : y[i]; +- z[i] = (i + half_size < full_size) ? ((z[i] >= z[i + half_size]) ? z[i] : z[i + half_size]) : z[i]; +- } +-} +- +-/* Find the smallest coordinate values */ +-extern "C" __global__ void findMin(float *x, float *y, float *z, int full_size, int half_size) +-{ +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = index; i < half_size; i += stride) { +- x[i] = (i + half_size < full_size) ? ((x[i] <= x[i + half_size]) ? x[i] : x[i + half_size]) : x[i]; +- y[i] = (i + half_size < full_size) ? ((y[i] <= y[i + half_size]) ? y[i] : y[i + half_size]) : y[i]; +- z[i] = (i + half_size < full_size) ? ((z[i] <= z[i + half_size]) ? z[i] : z[i + half_size]) : z[i]; +- } +-} +- +- +-void GVoxelGrid::findBoundaries() +-{ +- float *max_x, *max_y, *max_z, *min_x, *min_y, *min_z; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&max_x, sizeof(float) * points_num_)); +- rubis::sched::yield_gpu("228_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&max_y, sizeof(float) * points_num_)); +- rubis::sched::yield_gpu("229_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&max_z, sizeof(float) * points_num_)); +- rubis::sched::yield_gpu("230_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&min_x, sizeof(float) * points_num_)); +- rubis::sched::yield_gpu("231_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&min_y, sizeof(float) * points_num_)); +- rubis::sched::yield_gpu("232_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&min_z, sizeof(float) * points_num_)); +- rubis::sched::yield_gpu("233_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(max_x, x_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("234_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(max_y, y_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("235_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(max_z, z_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("236_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(min_x, x_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("237_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(min_y, y_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("238_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(min_z, z_, sizeof(float) * points_num_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("239_dtod"); +- +- int points_num = points_num_; +- +- while (points_num > 1) { +- int half_points_num = (points_num - 1) / 2 + 1; +- int block_x = (half_points_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : half_points_num; +- int grid_x = (half_points_num - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- findMax<<>>(max_x, max_y, max_z, points_num, half_points_num); +- rubis::sched::yield_gpu("240_findMax"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- findMin<<>>(min_x, min_y, min_z, points_num, half_points_num); +- rubis::sched::yield_gpu("241_findMin"); +- +- checkCudaErrors(cudaGetLastError()); +- +- points_num = half_points_num; +- } +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&max_x_, max_x, sizeof(float), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("242_dtoh"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&max_y_, max_y, sizeof(float), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("243_dtoh"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&max_z_, max_z, sizeof(float), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("244_dtoh"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&min_x_, min_x, sizeof(float), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("245_dtoh"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&min_y_, min_y, sizeof(float), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("246_dtoh"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(&min_z_, min_z, sizeof(float), cudaMemcpyDeviceToHost)); +- rubis::sched::yield_gpu("247_dtoh"); +- +- max_b_x_ = static_cast (floor(max_x_ / voxel_x_)); +- max_b_y_ = static_cast (floor(max_y_ / voxel_y_)); +- max_b_z_ = static_cast (floor(max_z_ / voxel_z_)); +- +- min_b_x_ = static_cast (floor(min_x_ / voxel_x_)); +- min_b_y_ = static_cast (floor(min_y_ / voxel_y_)); +- min_b_z_ = static_cast (floor(min_z_ / voxel_z_)); +- +- vgrid_x_ = max_b_x_ - min_b_x_ + 1; +- vgrid_y_ = max_b_y_ - min_b_y_ + 1; +- vgrid_z_ = max_b_z_ - min_b_z_ + 1; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_x)); +- rubis::sched::yield_gpu("248_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_y)); +- rubis::sched::yield_gpu("249_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_z)); +- rubis::sched::yield_gpu("250_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_x)); +- rubis::sched::yield_gpu("251_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_y)); +- rubis::sched::yield_gpu("252_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_z)); +- rubis::sched::yield_gpu("253_free"); +-} +- +-/* Find indexes idx, idy and idz of candidate voxels */ +-extern "C" __global__ void findBoundariesOfCandidateVoxels(float *x, float *y, float *z, +- float radius, int points_num, +- float voxel_x, float voxel_y, float voxel_z, +- int max_b_x, int max_b_y, int max_b_z, +- int min_b_x, int min_b_y, int min_b_z, +- int *max_vid_x, int *max_vid_y, int *max_vid_z, +- int *min_vid_x, int *min_vid_y, int *min_vid_z, +- int *candidate_voxel_per_point) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = id; i < points_num; i += stride) { +- float t_x = x[i]; +- float t_y = y[i]; +- float t_z = z[i]; +- +- int max_id_x = static_cast(floorf((t_x + radius) / voxel_x)); +- int max_id_y = static_cast(floorf((t_y + radius) / voxel_y)); +- int max_id_z = static_cast(floorf((t_z + radius) / voxel_z)); +- +- int min_id_x = static_cast(floorf((t_x - radius) / voxel_x)); +- int min_id_y = static_cast(floorf((t_y - radius) / voxel_y)); +- int min_id_z = static_cast(floorf((t_z - radius) / voxel_z)); +- +- /* Find intersection of the cube containing +- * the NN sphere of the point and the voxel grid +- */ +- max_id_x = (max_id_x > max_b_x) ? max_b_x - min_b_x : max_id_x - min_b_x; +- max_id_y = (max_id_y > max_b_y) ? max_b_y - min_b_y : max_id_y - min_b_y; +- max_id_z = (max_id_z > max_b_z) ? max_b_z - min_b_z : max_id_z - min_b_z; +- +- min_id_x = (min_id_x < min_b_x) ? 0 : min_id_x - min_b_x; +- min_id_y = (min_id_y < min_b_y) ? 0 : min_id_y - min_b_y; +- min_id_z = (min_id_z < min_b_z) ? 0 : min_id_z - min_b_z; +- +- int vx = max_id_x - min_id_x + 1; +- int vy = max_id_y - min_id_y + 1; +- int vz = max_id_z - min_id_z + 1; +- +- candidate_voxel_per_point[i] = (vx > 0 && vy > 0 && vz > 0) ? vx * vy * vz : 0; +- +- max_vid_x[i] = max_id_x; +- max_vid_y[i] = max_id_y; +- max_vid_z[i] = max_id_z; +- +- min_vid_x[i] = min_id_x; +- min_vid_y[i] = min_id_y; +- min_vid_z[i] = min_id_z; +- } +-} +- +-/* Write id of valid points to the output buffer */ +-extern "C" __global__ void collectValidPoints(int *valid_points_mark, int *valid_points_id, int *valid_points_location, int points_num) +-{ +- for (int index = threadIdx.x + blockIdx.x * blockDim.x; index < points_num; index += blockDim.x * gridDim.x) { +- if (valid_points_mark[index] != 0) { +- valid_points_id[valid_points_location[index]] = index; +- } +- } +-} +- +-/* Compute the global index of candidate voxels. +- * global index = idx + idy * grid size x + idz * grid_size x * grid size y */ +-extern "C" __global__ void updateCandidateVoxelIds(int points_num, +- int vgrid_x, int vgrid_y, int vgrid_z, +- int *max_vid_x, int *max_vid_y, int *max_vid_z, +- int *min_vid_x, int *min_vid_y, int *min_vid_z, +- int *starting_voxel_id, +- int *candidate_voxel_id) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = id; i < points_num; i += stride) { +- int max_id_x = max_vid_x[i]; +- int max_id_y = max_vid_y[i]; +- int max_id_z = max_vid_z[i]; +- +- int min_id_x = min_vid_x[i]; +- int min_id_y = min_vid_y[i]; +- int min_id_z = min_vid_z[i]; +- +- int write_location = starting_voxel_id[i]; +- +- for (int j = min_id_x; j <= max_id_x; j++) { +- for (int k = min_id_y; k <= max_id_y; k++) { +- for (int l = min_id_z; l <= max_id_z; l++) { +- candidate_voxel_id[write_location] = j + k * vgrid_x + l * vgrid_x * vgrid_y; +- write_location++; +- } +- } +- } +- } +-} +- +- +-/* Find out which voxels are really inside the radius. +- * This is done by comparing the distance between the centroid +- * of the voxel and the query point with the radius. +- * +- * The valid_voxel_mark store the result of the inspection, which is 0 +- * if the centroid is outside the radius and 1 otherwise. +- * +- * The valid_points_mark store the status of the inspection per point. +- * It is 0 if there is no voxels in the candidate list is truly a neighbor +- * of the point, and 1 otherwise. +- * +- * The valid_voxel_count store the number of true neighbor voxels. +- */ +-extern "C" __global__ void inspectCandidateVoxels(float *x, float *y, float *z, +- float radius, int max_nn, int points_num, +- double *centroid, int *points_per_voxel, int offset, +- int *starting_voxel_id, int *candidate_voxel_id, +- int *valid_voxel_mark, int *valid_voxel_count, int *valid_points_mark) +-{ +- int id = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = id; i < points_num; i += stride) { +- double t_x = static_cast(x[i]); +- double t_y = static_cast(y[i]); +- double t_z = static_cast(z[i]); +- +- int nn = 0; +- for (int j = starting_voxel_id[i]; j < starting_voxel_id[i + 1] && nn <= max_nn; j++) { +- int point_num = points_per_voxel[candidate_voxel_id[j]]; +- MatrixDevice centr(3, 1, offset, centroid + candidate_voxel_id[j]); +- +- double centroid_x = (point_num > 0) ? (t_x - centr(0)) : radius + 1; +- double centroid_y = (point_num > 0) ? (t_y - centr(1)) : 0; +- double centroid_z = (point_num > 0) ? (t_z - centr(2)) : 0; +- +- bool res = (norm3d(centroid_x, centroid_y, centroid_z) <= radius); +- +- valid_voxel_mark[j] = (res) ? 1 : 0; +- nn += (res) ? 1 : 0; +- } +- +- valid_voxel_count[i] = nn; +- valid_points_mark[i] = (nn > 0) ? 1 : 0; +- +- __syncthreads(); +- } +-} +- +-/* Write the id of valid voxels to the output buffer */ +-extern "C" __global__ void collectValidVoxels(int *valid_voxels_mark, int *candidate_voxel_id, int *output, int *writing_location, int candidate_voxel_num) +-{ +- for (int index = threadIdx.x + blockIdx.x * blockDim.x; index < candidate_voxel_num; index += blockDim.x * gridDim.x) { +- if (valid_voxels_mark[index] == 1) { +- output[writing_location[index]] = candidate_voxel_id[index]; +- } +- } +-} +- +-/* Write the number of valid voxel per point to the output buffer */ +-extern "C" __global__ void collectValidVoxelCount(int *input_valid_voxel_count, int *output_valid_voxel_count, int *writing_location, int points_num) +-{ +- for (int id = threadIdx.x + blockIdx.x * blockDim.x; id < points_num; id += blockDim.x * gridDim.x) { +- if (input_valid_voxel_count[id] != 0) +- output_valid_voxel_count[writing_location[id]] = input_valid_voxel_count[id]; +- } +-} +- +-template +-void GVoxelGrid::ExclusiveScan(T *input, int ele_num, T *sum) +-{ +- thrust::device_ptr dev_ptr(input); +- +- rubis::sched::request_gpu(); +- thrust::exclusive_scan(dev_ptr, dev_ptr + ele_num, dev_ptr); +- rubis::sched::yield_gpu("254_exclusive_scan"); +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- *sum = *(dev_ptr + ele_num - 1); +-} +- +-template +-void GVoxelGrid::ExclusiveScan(T *input, int ele_num) +-{ +- thrust::device_ptr dev_ptr(input); +- +- rubis::sched::request_gpu(); +- thrust::exclusive_scan(dev_ptr, dev_ptr + ele_num, dev_ptr); +- rubis::sched::yield_gpu("255_exclusive_scan"); +- +- checkCudaErrors(cudaDeviceSynchronize()); +-} +- +-void GVoxelGrid::radiusSearch(float *qx, float *qy, float *qz, int points_num, float radius, int max_nn, +- int **valid_points, int **starting_voxel_id, int **valid_voxel_id, +- int *valid_voxel_num, int *valid_points_num) +-{ +- //Testing input query points +- int block_x = (points_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : points_num; +- int grid_x = (points_num - 1) / block_x + 1; +- +- //Boundaries of candidate voxels per points +- int *max_vid_x, *max_vid_y, *max_vid_z; +- int *min_vid_x, *min_vid_y, *min_vid_z; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&max_vid_x, sizeof(int) * points_num)); +- rubis::sched::yield_gpu("256_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&max_vid_y, sizeof(int) * points_num)); +- rubis::sched::yield_gpu("257_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&max_vid_z, sizeof(int) * points_num)); +- rubis::sched::yield_gpu("258_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&min_vid_x, sizeof(int) * points_num)); +- rubis::sched::yield_gpu("259_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&min_vid_y, sizeof(int) * points_num)); +- rubis::sched::yield_gpu("260_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&min_vid_z, sizeof(int) * points_num)); +- rubis::sched::yield_gpu("261_cudaMalloc"); +- +- //Determine the number of candidate voxel per points +- int *candidate_voxel_num_per_point; +- int total_candidate_voxel_num; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&candidate_voxel_num_per_point, sizeof(int) * (points_num + 1))); +- rubis::sched::yield_gpu("262_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- findBoundariesOfCandidateVoxels<<>>(qx, qy, qz, radius, points_num, +- voxel_x_, voxel_y_, voxel_z_, +- max_b_x_, max_b_y_, max_b_z_, +- min_b_x_, min_b_y_, min_b_z_, +- max_vid_x, max_vid_y, max_vid_z, +- min_vid_x, min_vid_y, min_vid_z, +- candidate_voxel_num_per_point); +- rubis::sched::yield_gpu("263_findBoundariesOfCandidateVoxels"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- //Total candidate voxel num is determined by an exclusive scan on candidate_voxel_num_per_point +- ExclusiveScan(candidate_voxel_num_per_point, points_num + 1, &total_candidate_voxel_num); +- +- if (total_candidate_voxel_num <= 0) { +- std::cout << "No candidate voxel was found. Exiting..." << std::endl; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_x)); +- rubis::sched::yield_gpu("264_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_y)); +- rubis::sched::yield_gpu("265_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_z)); +- rubis::sched::yield_gpu("266_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_x)); +- rubis::sched::yield_gpu("267_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_y)); +- rubis::sched::yield_gpu("268_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_z)); +- rubis::sched::yield_gpu("269_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(candidate_voxel_num_per_point)); +- rubis::sched::yield_gpu("270_free"); +- +- valid_points = NULL; +- starting_voxel_id = NULL; +- valid_voxel_id = NULL; +- +- *valid_voxel_num = 0; +- *valid_points_num = 0; +- +- return; +- } +- +- +- //Determine the voxel id of candidate voxels +- int *candidate_voxel_id; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&candidate_voxel_id, sizeof(int) * total_candidate_voxel_num)); +- rubis::sched::yield_gpu("271_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- updateCandidateVoxelIds<<>>(points_num, vgrid_x_, vgrid_y_, vgrid_z_, +- max_vid_x, max_vid_y, max_vid_z, +- min_vid_x, min_vid_y, min_vid_z, +- candidate_voxel_num_per_point, candidate_voxel_id); +- rubis::sched::yield_gpu("272_updateCandidateVoxelIds"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- //Go through the candidate voxel id list and find out which voxels are really inside the radius +- int *valid_voxel_mark; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&valid_voxel_mark, sizeof(int) * total_candidate_voxel_num)); +- rubis::sched::yield_gpu("273_cudaMalloc"); +- +- int *valid_voxel_count; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&valid_voxel_count, sizeof(int) * (points_num + 1))); +- rubis::sched::yield_gpu("274_cudaMalloc"); +- +- int *valid_points_mark; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&valid_points_mark, sizeof(int) * points_num)); +- rubis::sched::yield_gpu("275_cudaMalloc"); +- +- block_x = (total_candidate_voxel_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : total_candidate_voxel_num; +- grid_x = (total_candidate_voxel_num - 1) / block_x + 1; +- +- ///CHECK VALID VOXEL COUNT AGAIN +- +- rubis::sched::request_gpu(); +- inspectCandidateVoxels<<>>(qx, qy, qz, radius, max_nn, points_num, +- centroid_, points_per_voxel_, voxel_num_, +- candidate_voxel_num_per_point, candidate_voxel_id, +- valid_voxel_mark, valid_voxel_count, valid_points_mark); +- rubis::sched::yield_gpu("276_inspectCandidateVoxels"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- //Collect valid points +- int *valid_points_location; +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&valid_points_location, sizeof(int) * (points_num + 1))); +- rubis::sched::yield_gpu("277_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(valid_points_location, 0, sizeof(int) * (points_num + 1))); +- rubis::sched::yield_gpu("278_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(valid_points_location, valid_points_mark, sizeof(int) * points_num, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("279_dtod"); +- +- //Writing location to the output buffer is determined by an exclusive scan +- ExclusiveScan(valid_points_location, points_num + 1, valid_points_num); +- +- if (*valid_points_num <= 0) { +- //std::cout << "No valid point was found. Exiting..." << std::endl; +- std::cout << "No valid point was found. Exiting...: " << *valid_points_num << std::endl; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_x)); +- rubis::sched::yield_gpu("280_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_y)); +- rubis::sched::yield_gpu("281_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_z)); +- rubis::sched::yield_gpu("282_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_x)); +- rubis::sched::yield_gpu("283_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_y)); +- rubis::sched::yield_gpu("284_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_z)); +- rubis::sched::yield_gpu("285_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(candidate_voxel_num_per_point)); +- rubis::sched::yield_gpu("286_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(candidate_voxel_id)); +- rubis::sched::yield_gpu("287_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_voxel_mark)); +- rubis::sched::yield_gpu("288_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_voxel_count)); +- rubis::sched::yield_gpu("289_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_points_mark)); +- rubis::sched::yield_gpu("290_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_points_location)); +- rubis::sched::yield_gpu("291_free"); +- +- valid_points = NULL; +- starting_voxel_id = NULL; +- valid_voxel_id = NULL; +- +- *valid_voxel_num = 0; +- *valid_points_num = 0; +- +- return; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(valid_points, sizeof(int) * (*valid_points_num))); +- rubis::sched::yield_gpu("292_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- collectValidPoints<<>>(valid_points_mark, *valid_points, valid_points_location, points_num); +- rubis::sched::yield_gpu("293_collectValidPoints"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(starting_voxel_id, sizeof(int) * (*valid_points_num + 1))); +- rubis::sched::yield_gpu("294_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- collectValidVoxelCount<<>>(valid_voxel_count, *starting_voxel_id, valid_points_location, points_num); +- rubis::sched::yield_gpu("295_collectValidVoxelCount"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- //Determine the starting location of voxels per points in the valid points list +- ExclusiveScan(*starting_voxel_id, *valid_points_num + 1, valid_voxel_num); +- +- //Collect valid voxels +- int *valid_voxel_location; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&valid_voxel_location, sizeof(int) * (total_candidate_voxel_num + 1))); +- rubis::sched::yield_gpu("296_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(valid_voxel_location, valid_voxel_mark, sizeof(int) * total_candidate_voxel_num, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("297_dtod"); +- +- ExclusiveScan(valid_voxel_location, total_candidate_voxel_num + 1, valid_voxel_num); +- +- if (*valid_voxel_num <= 0) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_x)); +- rubis::sched::yield_gpu("298_free"); +- +- max_vid_x = NULL; +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_y)); +- rubis::sched::yield_gpu("299_free"); +- +- max_vid_y = NULL; +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_z)); +- rubis::sched::yield_gpu("300_free"); +- +- max_vid_z = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_x)); +- rubis::sched::yield_gpu("301_free"); +- +- min_vid_x = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_y)); +- rubis::sched::yield_gpu("302_free"); +- +- min_vid_y = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_z)); +- rubis::sched::yield_gpu("303_free"); +- +- min_vid_z = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(candidate_voxel_num_per_point)); +- rubis::sched::yield_gpu("304_free"); +- +- candidate_voxel_num_per_point = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(candidate_voxel_id)); +- rubis::sched::yield_gpu("305_free"); +- +- candidate_voxel_id = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_voxel_mark)); +- rubis::sched::yield_gpu("306_free"); +- +- valid_voxel_mark = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_voxel_count)); +- rubis::sched::yield_gpu("307_free"); +- +- valid_voxel_count = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_points_mark)); +- rubis::sched::yield_gpu("308_free"); +- +- valid_points_mark = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_points_location)); +- rubis::sched::yield_gpu("309_free"); +- +- valid_points_location = NULL; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_voxel_location)); +- rubis::sched::yield_gpu("310_free"); +- +- valid_voxel_location = NULL; +- +- valid_points = NULL; +- starting_voxel_id = NULL; +- valid_voxel_id = NULL; +- +- *valid_voxel_num = 0; +- *valid_points_num = 0; +- } +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(valid_voxel_id, sizeof(int) * (*valid_voxel_num))); +- rubis::sched::yield_gpu("311_cudaMalloc"); +- +- block_x = (total_candidate_voxel_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : total_candidate_voxel_num; +- grid_x = (total_candidate_voxel_num - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- collectValidVoxels<<>>(valid_voxel_mark, candidate_voxel_id, *valid_voxel_id, valid_voxel_location, total_candidate_voxel_num); +- rubis::sched::yield_gpu("312_collectValidVoxels"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_x)); +- rubis::sched::yield_gpu("313_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_y)); +- rubis::sched::yield_gpu("314_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(max_vid_z)); +- rubis::sched::yield_gpu("315_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_x)); +- rubis::sched::yield_gpu("316_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_y)); +- rubis::sched::yield_gpu("317_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(min_vid_z)); +- rubis::sched::yield_gpu("318_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(candidate_voxel_num_per_point)); +- rubis::sched::yield_gpu("319_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(candidate_voxel_id)); +- rubis::sched::yield_gpu("320_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_voxel_mark)); +- rubis::sched::yield_gpu("321_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_points_mark)); +- rubis::sched::yield_gpu("322_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_voxel_count)); +- rubis::sched::yield_gpu("323_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_points_location)); +- rubis::sched::yield_gpu("324_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(valid_voxel_location)); +- rubis::sched::yield_gpu("325_free"); +-} +- +-/* Build parent nodes from child nodes of the octree */ +-extern "C" __global__ void buildParent(double *child_centroids, int *points_per_child, +- int child_grid_x, int child_grid_y, int child_grid_z, int child_num, +- double *parent_centroids, int *points_per_parent, +- int parent_grid_x, int parent_grid_y, int parent_grid_z) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int idy = threadIdx.y + blockIdx.y * blockDim.y; +- int idz = threadIdx.z + blockIdx.z * blockDim.z; +- +- if (idx < parent_grid_x && idy < parent_grid_y && idz < parent_grid_z) { +- int parent_idx = idx + idy * parent_grid_x + idz * parent_grid_x * parent_grid_y; +- MatrixDevice parent_centr(3, 1, parent_grid_x * parent_grid_y * parent_grid_z, parent_centroids + parent_idx); +- double pc0, pc1, pc2; +- int points_num = 0; +- double dpoints_num; +- +- pc0 = 0.0; +- pc1 = 0.0; +- pc2 = 0.0; +- +- for (int i = idx * 2; i < idx * 2 + 2 && i < child_grid_x; i++) { +- for (int j = idy * 2; j < idy * 2 + 2 && j < child_grid_y; j++) { +- for (int k = idz * 2; k < idz * 2 + 2 && k < child_grid_z; k++) { +- int child_idx = i + j * child_grid_x + k * child_grid_x * child_grid_y; +- MatrixDevice child_centr(3, 1, child_num, child_centroids + child_idx); +- int child_points = points_per_child[child_idx]; +- double dchild_points = static_cast(child_points); +- +- +- pc0 += (child_points > 0) ? dchild_points * child_centr(0) : 0.0; +- pc1 += (child_points > 0) ? dchild_points * child_centr(1) : 0.0; +- pc2 += (child_points > 0) ? dchild_points * child_centr(2) : 0.0; +- points_num += (child_points > 0) ? child_points : 0; +- +- __syncthreads(); +- } +- } +- } +- +- dpoints_num = static_cast(points_num); +- +- parent_centr(0) = (points_num <= 0) ? DBL_MAX : pc0 / dpoints_num; +- parent_centr(1) = (points_num <= 0) ? DBL_MAX : pc1 / dpoints_num; +- parent_centr(2) = (points_num <= 0) ? DBL_MAX : pc2 / dpoints_num; +- points_per_parent[parent_idx] = points_num; +- } +-} +- +-/* Compute the number of points per voxel using atomicAdd */ +-extern "C" __global__ void insertPointsToGrid(float *x, float *y, float *z, int points_num, +- int *points_per_voxel, int voxel_num, +- int vgrid_x, int vgrid_y, int vgrid_z, +- float voxel_x, float voxel_y, float voxel_z, +- int min_b_x, int min_b_y, int min_b_z) +-{ +- int index = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = index; i < points_num; i += stride) { +- float t_x = x[i]; +- float t_y = y[i]; +- float t_z = z[i]; +- int voxel_id = voxelId(t_x, t_y, t_z, voxel_x, voxel_y, voxel_z, min_b_x, min_b_y, min_b_z, vgrid_x, vgrid_y, vgrid_z); +- +- // Update number of points in the voxel +- int ptr_increment = (voxel_id < voxel_num) * voxel_id; // if (voxel_id < voxel_num), then use voxel_id +- int incremental_value = (voxel_id < voxel_num); +- //atomicAdd(points_per_voxel + voxel_id, 1); +- atomicAdd(points_per_voxel + ptr_increment, incremental_value); +- } +-} +- +-/* Rearrange points to locations corresponding to voxels */ +-extern "C" __global__ void scatterPointsToVoxels(float *x, float *y, float *z, int points_num, int voxel_num, +- float voxel_x, float voxel_y, float voxel_z, +- int min_b_x, int min_b_y, int min_b_z, +- int vgrid_x, int vgrid_y, int vgrid_z, +- int *writing_locations, int *point_ids) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = idx; i < points_num; i += stride) { +- int voxel_id = voxelId(x[i], y[i], z[i], voxel_x, voxel_y, voxel_z, +- min_b_x, min_b_y, min_b_z, vgrid_x, vgrid_y, vgrid_z); +- +- int ptr_increment = (voxel_id < voxel_num) * voxel_id; +- int incremental_value = (voxel_id < voxel_num); +- //int loc = atomicAdd(writing_locations + voxel_id, 1); +- int loc = atomicAdd(writing_locations + ptr_increment, incremental_value); +- +- point_ids[loc] = i; +- } +-} +- +-void GVoxelGrid::scatterPointsToVoxelGrid() +-{ +- if (starting_point_ids_ != NULL) { +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(starting_point_ids_)); +- rubis::sched::yield_gpu("326_free"); +- +- starting_point_ids_ = NULL; +- } +- +- if (point_ids_ != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(point_ids_)); +- rubis::sched::yield_gpu("327_free"); +- +- point_ids_ = NULL; +- } +- +- int block_x = (points_num_ > BLOCK_SIZE_X) ? BLOCK_SIZE_X : points_num_; +- int grid_x = (points_num_ - 1) / block_x + 1; +- +- rubis::sched::request_gpu(); +- insertPointsToGrid<<>>(x_, y_, z_, points_num_, points_per_voxel_, voxel_num_, +- vgrid_x_, vgrid_y_, vgrid_z_, +- voxel_x_, voxel_y_, voxel_z_, +- min_b_x_, min_b_y_, min_b_z_); +- rubis::sched::yield_gpu("328_insertPointsToGrid"); +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&starting_point_ids_, sizeof(int) * (voxel_num_ + 1))); +- rubis::sched::yield_gpu("329_cudaMalloc"); +- +- int *writing_location; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&writing_location, sizeof(int) * voxel_num_)); +- rubis::sched::yield_gpu("330_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(starting_point_ids_, points_per_voxel_, sizeof(int) * voxel_num_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("331_dtod"); +- +- ExclusiveScan(starting_point_ids_, voxel_num_ + 1); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemcpy(writing_location, starting_point_ids_, sizeof(int) * voxel_num_, cudaMemcpyDeviceToDevice)); +- rubis::sched::yield_gpu("332_dtod"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&point_ids_, sizeof(int) * points_num_)); +- rubis::sched::yield_gpu("333_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- scatterPointsToVoxels<<>>(x_, y_, z_, points_num_, voxel_num_, +- voxel_x_, voxel_y_, voxel_z_, +- min_b_x_, min_b_y_, min_b_z_, +- vgrid_x_, vgrid_y_, vgrid_z_, +- writing_location, point_ids_); +- rubis::sched::yield_gpu("334_scatterPointsToVoxels"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(writing_location)); +- rubis::sched::yield_gpu("335_free"); +-} +- +-void GVoxelGrid::buildOctree() +-{ +- for (unsigned int i = 1; i < octree_centroids_.size(); i++) { +- if (octree_centroids_[i] != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(octree_centroids_[i])); +- rubis::sched::yield_gpu("336_free"); +- +- octree_centroids_[i] = NULL; +- } +- if (octree_points_per_node_[i] != NULL) { +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(octree_points_per_node_[i])); +- rubis::sched::yield_gpu("337_free"); +- +- octree_points_per_node_[i] = NULL; +- } +- } +- +- octree_centroids_.clear(); +- octree_points_per_node_.clear(); +- octree_grid_size_.clear(); +- +- //Push leafs to the octree list +- octree_centroids_.push_back(centroid_); +- octree_points_per_node_.push_back(points_per_voxel_); +- OctreeGridSize grid_size; +- +- grid_size.size_x = vgrid_x_; +- grid_size.size_y = vgrid_y_; +- grid_size.size_z = vgrid_z_; +- +- octree_grid_size_.push_back(grid_size); +- +- int node_number = voxel_num_; +- int child_grid_x, child_grid_y, child_grid_z; +- int parent_grid_x, parent_grid_y, parent_grid_z; +- +- int i = 0; +- +- while (node_number > 8) { +- child_grid_x = octree_grid_size_[i].size_x; +- child_grid_y = octree_grid_size_[i].size_y; +- child_grid_z = octree_grid_size_[i].size_z; +- +- parent_grid_x = (child_grid_x - 1) / 2 + 1; +- parent_grid_y = (child_grid_y - 1) / 2 + 1; +- parent_grid_z = (child_grid_z - 1) / 2 + 1; +- +- node_number = parent_grid_x * parent_grid_y * parent_grid_z; +- +- double *parent_centroids; +- int *points_per_parent; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&parent_centroids, sizeof(double) * 3 * node_number)); +- rubis::sched::yield_gpu("338_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&points_per_parent, sizeof(int) * node_number)); +- rubis::sched::yield_gpu("339_cudaMalloc"); +- +- double *child_centroids = octree_centroids_[i]; +- int *points_per_child = octree_points_per_node_[i]; +- +- int block_x = (parent_grid_x > BLOCK_X) ? BLOCK_X : parent_grid_x; +- int block_y = (parent_grid_y > BLOCK_Y) ? BLOCK_Y : parent_grid_y; +- int block_z = (parent_grid_z > BLOCK_Z) ? BLOCK_Z : parent_grid_z; +- +- int grid_x = (parent_grid_x - 1) / block_x + 1; +- int grid_y = (parent_grid_y - 1) / block_y + 1; +- int grid_z = (parent_grid_z - 1) / block_z + 1; +- +- dim3 block(block_x, block_y, block_z); +- dim3 grid(grid_x, grid_y, grid_z); +- +- rubis::sched::request_gpu(); +- buildParent<<>>(child_centroids, points_per_child, +- child_grid_x, child_grid_y, child_grid_z, child_grid_x * child_grid_y * child_grid_z, +- parent_centroids, points_per_parent, +- parent_grid_x, parent_grid_y, parent_grid_z); +- rubis::sched::yield_gpu("340_buildParent"); +- checkCudaErrors(cudaGetLastError()); +- octree_centroids_.push_back(parent_centroids); +- octree_points_per_node_.push_back(points_per_parent); +- +- grid_size.size_x = parent_grid_x; +- grid_size.size_y = parent_grid_y; +- grid_size.size_z = parent_grid_z; +- +- octree_grid_size_.push_back(grid_size); +- +- i++; +- } +- +- checkCudaErrors(cudaDeviceSynchronize()); +-} +- +-/* Search for the nearest octree node */ +-extern "C" __global__ void nearestOctreeNodeSearch(float *x, float *y, float *z, +- int *vid_x, int *vid_y, int *vid_z, +- int points_num, +- double *centroids, int *points_per_node, +- int vgrid_x, int vgrid_y, int vgrid_z, int node_num) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = idx; i < points_num; i += stride) { +- int vx = vid_x[i]; +- int vy = vid_y[i]; +- int vz = vid_z[i]; +- double min_dist = DBL_MAX; +- double t_x = static_cast(x[i]); +- double t_y = static_cast(y[i]); +- double t_z = static_cast(z[i]); +- double cur_dist; +- +- int out_x, out_y, out_z; +- +- out_x = vx; +- out_y = vy; +- out_z = vz; +- +- double tmp_x, tmp_y, tmp_z; +- +- for (int j = vx * 2; j < vx * 2 + 2 && j < vgrid_x; j++) { +- for (int k = vy * 2; k < vy * 2 + 2 && k < vgrid_y; k++) { +- for (int l = vz * 2; l < vz * 2 + 2 && l < vgrid_z; l++) { +- int node_id = j + k * vgrid_x + l * vgrid_x * vgrid_y; +- MatrixDevice node_centr(3, 1, node_num, centroids + node_id); +- int points = points_per_node[node_id]; +- +- tmp_x = (points > 0) ? node_centr(0) - t_x : DBL_MAX; +- tmp_y = (points > 0) ? node_centr(1) - t_y : 0.0; +- tmp_z = (points > 0) ? node_centr(2) - t_z : 0.0; +- +- cur_dist = norm3d(tmp_x, tmp_y, tmp_z); +- bool res = (cur_dist < min_dist); +- +- out_x = (res) ? j : out_x; +- out_y = (res) ? k : out_y; +- out_z = (res) ? l : out_z; +- +- min_dist = (res) ? cur_dist : min_dist; +- } +- } +- } +- +- vid_x[i] = out_x; +- vid_y[i] = out_y; +- vid_z[i] = out_z; +- } +-} +- +-/* Search for the nearest point from nearest voxel */ +-extern "C" __global__ void nearestPointSearch(float *qx, float *qy, float *qz, int qpoints_num, +- float *rx, float *ry, float *rz, int rpoints_num, +- int *vid_x, int *vid_y, int *vid_z, +- int vgrid_x, int vgrid_y, int vgrid_z, int voxel_num, +- int *starting_point_id, int *point_id, double *min_distance) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = idx; i < qpoints_num; i += stride) { +- int voxel_id = vid_x[i] + vid_y[i] * vgrid_x + vid_z[i] * vgrid_x * vgrid_y; +- float cor_qx = qx[i]; +- float cor_qy = qy[i]; +- float cor_qz = qz[i]; +- float min_dist = FLT_MAX; +- +- for (int j = starting_point_id[voxel_id]; j < starting_point_id[voxel_id + 1]; j++) { +- int pid = point_id[j]; +- float cor_rx = rx[pid]; +- float cor_ry = ry[pid]; +- float cor_rz = rz[pid]; +- +- cor_rx -= cor_qx; +- cor_ry -= cor_qy; +- cor_rz -= cor_qz; +- +- min_dist = fminf(norm3df(cor_rx, cor_ry, cor_rz), min_dist); +- } +- +- min_distance[i] = static_cast(min_dist); +- } +-} +- +-/* Verify if min distances are really smaller than or equal to max_range */ +-extern "C" __global__ void verifyDistances(int *valid_distance, double *min_distance, double max_range, int points_num) +-{ +- int idx = threadIdx.x + blockIdx.x * blockDim.x; +- int stride = blockDim.x * gridDim.x; +- +- for (int i = idx; i < points_num; i += stride) { +- bool check = (min_distance[i] <= max_range); +- +- valid_distance[i] = (check) ? 1 : 0; +- +- if (!check) { +- min_distance[i] = 0; +- } +- } +-} +- +-void GVoxelGrid::nearestNeighborSearch(float *trans_x, float *trans_y, float *trans_z, int point_num, int *valid_distance, double *min_distance, float max_range) +-{ +- +- int *vid_x, *vid_y, *vid_z; +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&vid_x, sizeof(int) * point_num)); +- rubis::sched::yield_gpu("338_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&vid_y, sizeof(int) * point_num)); +- rubis::sched::yield_gpu("339_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMalloc(&vid_z, sizeof(int) * point_num)); +- rubis::sched::yield_gpu("340_cudaMalloc"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(vid_x, 0, sizeof(int) * point_num)); +- rubis::sched::yield_gpu("341_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(vid_y, 0, sizeof(int) * point_num)); +- rubis::sched::yield_gpu("342_cudaMemset"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaMemset(vid_z, 0, sizeof(int) * point_num)); +- rubis::sched::yield_gpu("343_cudaMemset"); +- +- checkCudaErrors(cudaDeviceSynchronize()); +- +- int block_x = (point_num > BLOCK_SIZE_X) ? BLOCK_SIZE_X : point_num; +- int grid_x = (point_num - 1) / block_x + 1; +- +- // Go through top of the octree to the bottom +- for (int i = octree_centroids_.size() - 1; i >= 0; i--) { +- double *centroids = octree_centroids_[i]; +- int *points_per_node = octree_points_per_node_[i]; +- int vgrid_x = octree_grid_size_[i].size_x; +- int vgrid_y = octree_grid_size_[i].size_y; +- int vgrid_z = octree_grid_size_[i].size_z; +- int node_num = vgrid_x * vgrid_y * vgrid_z; +- +- rubis::sched::request_gpu(); +- nearestOctreeNodeSearch<<>>(trans_x, trans_y, trans_z, +- vid_x, vid_y, vid_z, +- point_num, +- centroids, points_per_node, +- vgrid_x, vgrid_y, vgrid_z, node_num); +- rubis::sched::yield_gpu("344_nearestOctreeNodeSearch"); +- +- checkCudaErrors(cudaGetLastError()); +- } +- +- rubis::sched::request_gpu(); +- nearestPointSearch<<>>(trans_x, trans_y, trans_z, point_num, +- x_, y_, z_, points_num_, +- vid_x, vid_y, vid_z, +- vgrid_x_, vgrid_y_, vgrid_z_, voxel_num_, +- starting_point_ids_, point_ids_, +- min_distance); +- rubis::sched::yield_gpu("345_nearestPointSearch"); +- +- checkCudaErrors(cudaGetLastError()); +- +- rubis::sched::request_gpu(); +- verifyDistances<<>>(valid_distance, min_distance, max_range, point_num); +- rubis::sched::yield_gpu("346_verifyDistances"); +- +- checkCudaErrors(cudaGetLastError()); +- checkCudaErrors(cudaDeviceSynchronize()); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(vid_x)); +- rubis::sched::yield_gpu("347_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(vid_y)); +- rubis::sched::yield_gpu("348_free"); +- +- rubis::sched::request_gpu(); +- checkCudaErrors(cudaFree(vid_z)); +- rubis::sched::yield_gpu("349_free"); +-} +- +-} +diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter.launch b/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter.launch +index 46a29468..73d458d3 100644 +--- a/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter.launch ++++ b/autoware.ai/src/autoware/core_perception/points_downsampler/launch/voxel_grid_filter.launch +@@ -4,13 +4,11 @@ + + + +- + + + + + + +- + + +diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp +index 2aa58dfa..6958f845 100644 +--- a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp ++++ b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.cpp +@@ -62,7 +62,7 @@ static void config_callback(const autoware_config_msgs::ConfigVoxelGridFilter::C + } + + inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstPtr& input) +-{ ++{ + pcl::PointCloud scan; + pcl::fromROSMsg(*input, scan); + +@@ -97,12 +97,10 @@ inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstP + filtered_msg.header = input->header; + filtered_points_pub.publish(filtered_msg); + +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_msgs::PointCloud2 rubis_filtered_msg; +- rubis_filtered_msg.msg = filtered_msg; +- rubis_filtered_msg.instance = rubis::instance_; +- rubis_filtered_points_pub.publish(rubis_filtered_msg); +- } ++ rubis_msgs::PointCloud2 rubis_filtered_msg; ++ rubis_filtered_msg.msg = filtered_msg; ++ rubis_filtered_msg.instance = rubis::instance_; ++ rubis_filtered_points_pub.publish(rubis_filtered_msg); + + points_downsampler_info_msg.header = input->header; + points_downsampler_info_msg.filter_name = "voxel_grid_filter"; +@@ -137,22 +135,24 @@ inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstP + << points_downsampler_info_msg.exe_time << "," + << std::endl; + } +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + } + + static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) + { +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + publish_filtered_cloud(input); + } + + static void rubis_scan_callback(const rubis_msgs::PointCloud2::ConstPtr& _input) + { ++ rubis::start_task_profiling(); ++ + sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared(_input->msg); + rubis::instance_ = _input->instance; + publish_filtered_cloud(input); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + int main(int argc, char** argv) +@@ -162,15 +162,6 @@ int main(int argc, char** argv) + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- + private_nh.param("input_topic_name", input_topic_name_, std::string("points_raw")); + private_nh.param("output_topic_name", output_topic_name_, std::string("filtered_points")); + private_nh.getParam("output_log", _output_log); +@@ -185,23 +176,32 @@ int main(int argc, char** argv) + private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); + private_nh.param("leaf_size", voxel_leaf_size, 0.1); + ++ // Scheduling & Profiling Setup + std::string node_name = ros::this_node::getName(); +- private_nh.param(node_name+"/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param(node_name+"/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voxel_grid_filter.csv"); ++ std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voexl_grid_filter.csv"); ++ ++ int rate; + private_nh.param(node_name+"/rate", rate, 10); +- private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10); +- private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); +- private_nh.param(node_name+"/instance_mode", rubis::instance_mode_, 0); + +- /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ + + // Publishers + filtered_points_pub = nh.advertise(output_topic_name_, 10); +- if(rubis::instance_mode_) +- rubis_filtered_points_pub = nh.advertise("/rubis_" + output_topic_name_, 10); ++ rubis_filtered_points_pub = nh.advertise("/rubis_" + output_topic_name_, 10); + points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); + + // Subscribers +@@ -209,47 +209,9 @@ int main(int argc, char** argv) + // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback); + ros::Subscriber scan_sub; + +- if(rubis::instance_mode_) scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 10, rubis_scan_callback); +- else scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback); +- +- /* RT Scheduling setup */ +- // ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 1, config_callback); // origin 10 +- // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 1, scan_callback); // origin 10 +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 1, rubis_scan_callback); // Def: 10 + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ ros::spin(); + + return 0; + } +diff --git a/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp +new file mode 100644 +index 00000000..b931fd74 +--- /dev/null ++++ b/autoware.ai/src/autoware/core_perception/points_downsampler/nodes/voxel_grid_filter/voxel_grid_filter.origin.cpp +@@ -0,0 +1,211 @@ ++/* ++ * Copyright 2015-2019 Autoware Foundation. All rights reserved. ++ * ++ * Licensed under the Apache License, Version 2.0 (the "License"); ++ * you may not use this file except in compliance with the License. ++ * You may obtain a copy of the License at ++ * ++ * http://www.apache.org/licenses/LICENSE-2.0 ++ * ++ * Unless required by applicable law or agreed to in writing, software ++ * distributed under the License is distributed on an "AS IS" BASIS, ++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ++ * See the License for the specific language governing permissions and ++ * limitations under the License. ++ */ ++ ++#include ++#include ++ ++#include ++#include ++#include ++ ++#include "autoware_config_msgs/ConfigVoxelGridFilter.h" ++ ++#include ++ ++#include ++ ++#include "points_downsampler.h" ++ ++#include ++#include ++ ++#define SPIN_PROFILING ++ ++#define MAX_MEASUREMENT_RANGE 200.0 ++ ++ros::Publisher filtered_points_pub; ++ros::Publisher rubis_filtered_points_pub; ++ ++// Leaf size of VoxelGrid filter. ++static double voxel_leaf_size = 2.0; ++ ++static ros::Publisher points_downsampler_info_pub; ++static points_downsampler::PointsDownsamplerInfo points_downsampler_info_msg; ++ ++static std::chrono::time_point filter_start, filter_end; ++ ++static bool _output_log = false; ++static std::ofstream ofs; ++static std::string filename; ++ ++static std::string input_topic_name_; ++static std::string output_topic_name_; ++static double measurement_range = MAX_MEASUREMENT_RANGE; ++ ++static void config_callback(const autoware_config_msgs::ConfigVoxelGridFilter::ConstPtr& input) ++{ ++ voxel_leaf_size = input->voxel_leaf_size; ++ measurement_range = input->measurement_range; ++} ++ ++inline static void publish_filtered_cloud(const sensor_msgs::PointCloud2::ConstPtr& input) ++{ ++ pcl::PointCloud scan; ++ pcl::fromROSMsg(*input, scan); ++ ++ if(measurement_range != MAX_MEASUREMENT_RANGE){ ++ scan = removePointsByRange(scan, 0, measurement_range); ++ } ++ ++ pcl::PointCloud::Ptr scan_ptr(new pcl::PointCloud(scan)); ++ pcl::PointCloud::Ptr filtered_scan_ptr(new pcl::PointCloud()); ++ ++ sensor_msgs::PointCloud2 filtered_msg; ++ ++ filter_start = std::chrono::system_clock::now(); ++ ++ // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL) ++ if (voxel_leaf_size >= 0.1) ++ { ++ // Downsampling the velodyne scan using VoxelGrid filter ++ pcl::VoxelGrid voxel_grid_filter; ++ voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); ++ voxel_grid_filter.setInputCloud(scan_ptr); ++ voxel_grid_filter.filter(*filtered_scan_ptr); ++ pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); ++ } ++ else ++ { ++ pcl::toROSMsg(*scan_ptr, filtered_msg); ++ } ++ ++ filter_end = std::chrono::system_clock::now(); ++ ++ filtered_msg.header = input->header; ++ filtered_points_pub.publish(filtered_msg); ++ ++ rubis_msgs::PointCloud2 rubis_filtered_msg; ++ rubis_filtered_msg.msg = filtered_msg; ++ rubis_filtered_msg.instance = rubis::instance_; ++ rubis_filtered_points_pub.publish(rubis_filtered_msg); ++ ++ points_downsampler_info_msg.header = input->header; ++ points_downsampler_info_msg.filter_name = "voxel_grid_filter"; ++ points_downsampler_info_msg.measurement_range = measurement_range; ++ points_downsampler_info_msg.original_points_size = scan.size(); ++ if (voxel_leaf_size >= 0.1) ++ { ++ points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); ++ } ++ else ++ { ++ points_downsampler_info_msg.filtered_points_size = scan_ptr->size(); ++ } ++ points_downsampler_info_msg.original_ring_size = 0; ++ points_downsampler_info_msg.filtered_ring_size = 0; ++ points_downsampler_info_msg.exe_time = std::chrono::duration_cast(filter_end - filter_start).count() / 1000.0; ++ points_downsampler_info_pub.publish(points_downsampler_info_msg); ++ ++ if(_output_log == true){ ++ if(!ofs){ ++ std::cerr << "Could not open " << filename << "." << std::endl; ++ exit(1); ++ } ++ ofs << points_downsampler_info_msg.header.seq << "," ++ << points_downsampler_info_msg.header.stamp << "," ++ << points_downsampler_info_msg.header.frame_id << "," ++ << points_downsampler_info_msg.filter_name << "," ++ << points_downsampler_info_msg.original_points_size << "," ++ << points_downsampler_info_msg.filtered_points_size << "," ++ << points_downsampler_info_msg.original_ring_size << "," ++ << points_downsampler_info_msg.filtered_ring_size << "," ++ << points_downsampler_info_msg.exe_time << "," ++ << std::endl; ++ } ++ ++} ++ ++static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) ++{ ++ rubis::instance_ = 0; ++ publish_filtered_cloud(input); ++} ++ ++static void rubis_scan_callback(const rubis_msgs::PointCloud2::ConstPtr& _input) ++{ ++ rubis::start_task_profiling(); ++ ++ sensor_msgs::PointCloud2::ConstPtr input = boost::make_shared(_input->msg); ++ rubis::instance_ = _input->instance; ++ publish_filtered_cloud(input); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++} ++ ++int main(int argc, char** argv) ++{ ++ ros::init(argc, argv, "rubis_voxel_grid_filter"); ++ ++ ros::NodeHandle nh; ++ ros::NodeHandle private_nh("~"); ++ ++ // Scheduling Setup ++ std::string task_response_time_filename; ++ int rate; ++ double task_minimum_inter_release_time; ++ double task_execution_time; ++ double task_relative_deadline; ++ ++ private_nh.param("input_topic_name", input_topic_name_, std::string("points_raw")); ++ private_nh.param("output_topic_name", output_topic_name_, std::string("filtered_points")); ++ private_nh.getParam("output_log", _output_log); ++ if(_output_log == true){ ++ char buffer[80]; ++ std::time_t now = std::time(NULL); ++ std::tm *pnow = std::localtime(&now); ++ std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow); ++ filename = "voxel_grid_filter_" + std::string(buffer) + ".csv"; ++ ofs.open(filename.c_str(), std::ios::app); ++ } ++ private_nh.param("measurement_range", measurement_range, MAX_MEASUREMENT_RANGE); ++ private_nh.param("leaf_size", voxel_leaf_size, 0.1); ++ ++ std::string node_name = ros::this_node::getName(); ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/voxel_grid_filter.csv"); ++ private_nh.param(node_name+"/rate", rate, 10); ++ private_nh.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); ++ private_nh.param(node_name+"/task_execution_time", task_execution_time, (double)10); ++ private_nh.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); ++ ++ /* For Task scheduling */ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ // Publishers ++ filtered_points_pub = nh.advertise(output_topic_name_, 10); ++ rubis_filtered_points_pub = nh.advertise("/rubis_" + output_topic_name_, 10); ++ points_downsampler_info_pub = nh.advertise("/points_downsampler_info", 1000); ++ ++ // Subscribers ++ ros::Subscriber config_sub = nh.subscribe("config/voxel_grid_filter", 10, config_callback); ++ // ros::Subscriber scan_sub = nh.subscribe(input_topic_name_, 10, scan_callback); ++ ros::Subscriber scan_sub; ++ ++ scan_sub = nh.subscribe("/rubis_"+input_topic_name_, 10, rubis_scan_callback); ++ ++ ros::spin(); ++ ++ return 0; ++} +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h b/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h +index 18298682..5551e95e 100644 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/include/points_preprocessor/ray_ground_filter/ray_ground_filter.h +@@ -78,8 +78,6 @@ private: + std::vector colors_; + const size_t color_num_ = 60; // different number of color to generate + +- int instance_mode_ = 0; +- + struct PointXYZIRTColor + { + pcl::PointXYZI point; +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch +index d4fcb70e..5418c56f 100755 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter.launch +@@ -14,7 +14,6 @@ + + + +- + + + +@@ -31,6 +30,5 @@ + + + +- + + +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch +index 25fbed0c..eb60b4a3 100755 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/launch/ray_ground_filter_params.launch +@@ -14,7 +14,6 @@ + + + +- + + + +@@ -31,6 +30,5 @@ + + + +- + + +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp +index 92454425..d205d30f 100644 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/compare_map_filter/compare_map_filter.cpp +@@ -221,8 +221,7 @@ void CompareMapFilter::sensorPointsCallback(const sensor_msgs::PointCloud2::Cons + return; + } + unmatch_points_pub_.publish(sensorTF_unmatch_cloud_msg); +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + } + + void CompareMapFilter::searchMatchingCloud(const pcl::PointCloud::Ptr in_cloud_ptr, +diff --git a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp +index 005e14fb..e5b8bdca 100644 +--- a/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp ++++ b/autoware.ai/src/autoware/core_perception/points_preprocessor/nodes/ray_ground_filter/ray_ground_filter.cpp +@@ -379,8 +379,6 @@ inline void RayGroundFilter::PublishFilteredClouds(const sensor_msgs::PointCloud + publish_cloud(ground_points_pub_, ground_cloud_ptr, in_sensor_cloud->header); + publish_cloud(groundless_points_pub_, no_ground_cloud_ptr, in_sensor_cloud->header); + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; + } + + void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_sensor_cloud) +@@ -391,9 +389,11 @@ void RayGroundFilter::CloudCallback(const sensor_msgs::PointCloud2ConstPtr& in_s + + void RayGroundFilter::RubisCloudCallback(const rubis_msgs::PointCloud2ConstPtr in_rubis_cloud) + { ++ rubis::start_task_profiling(); + sensor_msgs::PointCloud2ConstPtr in_sensor_cloud = boost::make_shared(in_rubis_cloud->msg); + rubis::instance_ = in_rubis_cloud->instance; + PublishFilteredClouds(in_sensor_cloud); ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + RayGroundFilter::RayGroundFilter() : node_handle_("~"), tf_listener_(tf_buffer_) +@@ -405,15 +405,6 @@ RayGroundFilter::RayGroundFilter() : node_handle_("~"), tf_listener_(tf_buffer_) + + void RayGroundFilter::Run() + { +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- + // Model | Horizontal | Vertical | FOV(Vertical) degrees / rads + // ---------------------------------------------------------- + // HDL-64 |0.08-0.35(0.32) | 0.4 | -24.9 <=x<=2.0 (26.9 / 0.47) +@@ -469,51 +460,31 @@ void RayGroundFilter::Run() + groundless_points_pub_ = node_handle_.advertise(no_ground_topic, 2); + ground_points_pub_ = node_handle_.advertise(ground_topic, 2); + ++ // Scheduling & Profiling Setup + std::string node_name = ros::this_node::getName(); +- node_handle_.param(node_name+"/task_scheduling_flag", task_scheduling_flag, 0); +- node_handle_.param(node_name+"/task_profiling_flag", task_profiling_flag, 0); ++ std::string task_response_time_filename; + node_handle_.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/ray_ground_filter.csv"); +- node_handle_.param(node_name+"/rate", rate, 10); +- node_handle_.param(node_name+"/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- node_handle_.param(node_name+"/task_execution_time", task_execution_time, (double)10); +- node_handle_.param(node_name+"/task_relative_deadline", task_relative_deadline, (double)10); +- node_handle_.param(node_name+"/instance_mode", instance_mode_, 0); + +- points_node_sub_ = node_handle_.subscribe("/rubis_"+input_point_topic_.substr(1), 1, &RayGroundFilter::RubisCloudCallback, this); +- +- ROS_INFO("Ready"); ++ int rate; ++ node_handle_.param(node_name+"/rate", rate, 10); + +- /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ node_handle_.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ node_handle_.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ node_handle_.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ node_handle_.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ node_handle_.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_) break; +- ros::spinOnce(); +- r.sleep(); +- } ++ rubis::init_task_profiling(task_response_time_filename); + +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ points_node_sub_ = node_handle_.subscribe("/rubis_"+input_point_topic_.substr(1), 1, &RayGroundFilter::RubisCloudCallback, this); + +- ros::spinOnce(); ++ ROS_INFO("Ready"); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ ros::spin(); + } +diff --git a/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp b/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp +index 1ced3fe7..65943460 100644 +--- a/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp ++++ b/autoware.ai/src/autoware/core_perception/range_vision_fusion/src/range_vision_fusion.cpp +@@ -717,15 +717,13 @@ ROSRangeVisionFusionApp::Run() + ros::NodeHandle private_node_handle("~"); + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; + double task_execution_time; + double task_relative_deadline; + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + tf::TransformListener transform_listener; + +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu +index c7f2ef0b..83a0a55f 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/activation_kernels.cu +@@ -156,10 +156,8 @@ __global__ void binary_gradient_array_kernel(float *x, float *dy, int n, int s, + + extern "C" void binary_gradient_array_gpu(float *x, float *dx, int n, int size, BINARY_ACTIVATION a, float *y) + { +- request_gpu(); +- binary_gradient_array_kernel<<>>(x, dx, n/2, size, a, y); +- yield_gpu_with_remark("binary_gradient_array_kernel"); +- check_error(cudaPeekAtLastError()); ++ binary_gradient_array_kernel<<>>(x, dx, n/2, size, a, y); ++ check_error(cudaPeekAtLastError()); + } + __global__ void binary_activate_array_kernel(float *x, int n, int s, BINARY_ACTIVATION a, float *y) + { +@@ -173,9 +171,7 @@ __global__ void binary_activate_array_kernel(float *x, int n, int s, BINARY_ACTI + + extern "C" void binary_activate_array_gpu(float *x, int n, int size, BINARY_ACTIVATION a, float *y) + { +- request_gpu(); +- binary_activate_array_kernel<<>>(x, n/2, size, a, y); +- yield_gpu_with_remark("binary_activate_array_kernel"); ++ binary_activate_array_kernel<<>>(x, n/2, size, a, y); + check_error(cudaPeekAtLastError()); + } + +@@ -193,16 +189,12 @@ __global__ void gradient_array_kernel(float *x, int n, ACTIVATION a, float *delt + + extern "C" void activate_array_gpu(float *x, int n, ACTIVATION a) + { +- request_gpu(); +- activate_array_kernel<<>>(x, n, a); +- yield_gpu_with_remark("activate_array_kernel"); ++ activate_array_kernel<<>>(x, n, a); + check_error(cudaPeekAtLastError()); + } + + extern "C" void gradient_array_gpu(float *x, int n, ACTIVATION a, float *delta) + { +- request_gpu(); +- gradient_array_kernel<<>>(x, n, a, delta); +- yield_gpu_with_remark("gradient_array_kernel"); ++ gradient_array_kernel<<>>(x, n, a, delta); + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu +index 5a0473bc..65a521a2 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/avgpool_layer_kernels.cu +@@ -47,10 +47,7 @@ extern "C" void forward_avgpool_layer_gpu(avgpool_layer layer, network net) + { + size_t n = layer.c*layer.batch; + +- request_gpu(); +- forward_avgpool_layer_kernel<<>>(n, layer.w, layer.h, layer.c, net.input_gpu, layer.output_gpu); +- yield_gpu_with_remark("forward_avgpool_layer_kernel"); +- ++ forward_avgpool_layer_kernel<<>>(n, layer.w, layer.h, layer.c, net.input_gpu, layer.output_gpu); + check_error(cudaPeekAtLastError()); + } + +@@ -58,9 +55,7 @@ extern "C" void backward_avgpool_layer_gpu(avgpool_layer layer, network net) + { + size_t n = layer.c*layer.batch; + +- request_gpu(); +- backward_avgpool_layer_kernel<<>>(n, layer.w, layer.h, layer.c, net.delta_gpu, layer.delta_gpu); +- yield_gpu_with_remark("forward_avgpool_layer_kernel"); ++ backward_avgpool_layer_kernel<<>>(n, layer.w, layer.h, layer.c, net.delta_gpu, layer.delta_gpu); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu +index 95c61fd3..0ad326d9 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/blas_kernels.cu +@@ -23,9 +23,7 @@ void scale_bias_gpu(float *output, float *biases, int batch, int n, int size) + dim3 dimGrid((size-1)/BLOCK + 1, n, batch); + dim3 dimBlock(BLOCK, 1, 1); + +- request_gpu(); +- scale_bias_kernel<<>>(output, biases, n, size); +- yield_gpu_with_remark("scale_bias_kernel"); ++ scale_bias_kernel<<>>(output, biases, n, size); + + check_error(cudaPeekAtLastError()); + } +@@ -52,9 +50,7 @@ __global__ void backward_scale_kernel(float *x_norm, float *delta, int batch, in + + void backward_scale_gpu(float *x_norm, float *delta, int batch, int n, int size, float *scale_updates) + { +- request_gpu(); +- backward_scale_kernel<<>>(x_norm, delta, batch, n, size, scale_updates); +- yield_gpu_with_remark("backward_scale_kernel"); ++ backward_scale_kernel<<>>(x_norm, delta, batch, n, size, scale_updates); + + check_error(cudaPeekAtLastError()); + } +@@ -76,9 +72,7 @@ void add_bias_gpu(float *output, float *biases, int batch, int n, int size) + { + int num = n*size*batch; + +- request_gpu(); +- add_bias_kernel<<>>(output, biases, batch, n, size); +- yield_gpu_with_remark("add_bias_kernel"); ++ add_bias_kernel<<>>(output, biases, batch, n, size); + + check_error(cudaPeekAtLastError()); + } +@@ -119,13 +113,9 @@ __global__ void backward_bias_kernel(float *bias_updates, float *delta, int batc + void backward_bias_gpu(float *bias_updates, float *delta, int batch, int n, int size) + { + if(size == 1){ +- request_gpu(); +- backward_bias_conn_kernel<<>>(bias_updates, delta, batch, n); +- yield_gpu_with_remark("backward_bias_conn_kernel"); ++ backward_bias_conn_kernel<<>>(bias_updates, delta, batch, n); + }else{ +- request_gpu(); +- backward_bias_kernel<<>>(bias_updates, delta, batch, n, size); +- yield_gpu_with_remark("backward_bias_kernel"); ++ backward_bias_kernel<<>>(bias_updates, delta, batch, n, size); + } + check_error(cudaPeekAtLastError()); + } +@@ -186,9 +176,7 @@ __global__ void adam_kernel(int N, float *x, float *m, float *v, float B1, float + + extern "C" void adam_gpu(int n, float *x, float *m, float *v, float B1, float B2, float rate, float eps, int t) + { +- request_gpu(); +- adam_kernel<<>>(n, x, m, v, B1, B2, rate, eps, t); +- yield_gpu_with_remark("adam_kernel"); ++ adam_kernel<<>>(n, x, m, v, B1, B2, rate, eps, t); + + check_error(cudaPeekAtLastError()); + } +@@ -229,9 +217,7 @@ extern "C" void normalize_delta_gpu(float *x, float *mean, float *variance, floa + { + size_t N = batch*filters*spatial; + +- request_gpu(); +- normalize_delta_kernel<<>>(N, x, mean, variance, mean_delta, variance_delta, batch, filters, spatial, delta); +- yield_gpu_with_remark("normalize_delta_kernel"); ++ normalize_delta_kernel<<>>(N, x, mean, variance, mean_delta, variance_delta, batch, filters, spatial, delta); + + check_error(cudaPeekAtLastError()); + } +@@ -339,27 +325,21 @@ __global__ void mean_delta_kernel(float *delta, float *variance, int batch, int + + extern "C" void mean_delta_gpu(float *delta, float *variance, int batch, int filters, int spatial, float *mean_delta) + { +- request_gpu(); +- mean_delta_kernel<<>>(delta, variance, batch, filters, spatial, mean_delta); +- yield_gpu_with_remark("mean_delta_kernel"); ++ mean_delta_kernel<<>>(delta, variance, batch, filters, spatial, mean_delta); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void fast_mean_delta_gpu(float *delta, float *variance, int batch, int filters, int spatial, float *mean_delta) + { +- request_gpu(); +- fast_mean_delta_kernel<<>>(delta, variance, batch, filters, spatial, mean_delta); +- yield_gpu_with_remark("fast_mean_delta_kernel"); ++ fast_mean_delta_kernel<<>>(delta, variance, batch, filters, spatial, mean_delta); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void fast_variance_delta_gpu(float *x, float *delta, float *mean, float *variance, int batch, int filters, int spatial, float *variance_delta) + { +- request_gpu(); +- fast_variance_delta_kernel<<>>(x, delta, mean, variance, batch, filters, spatial, variance_delta); +- yield_gpu_with_remark("fast_variance_delta_kernel"); ++ fast_variance_delta_kernel<<>>(x, delta, mean, variance, batch, filters, spatial, variance_delta); + + check_error(cudaPeekAtLastError()); + } +@@ -495,9 +475,7 @@ extern "C" void normalize_gpu(float *x, float *mean, float *variance, int batch, + { + size_t N = batch*filters*spatial; + +- request_gpu(); +- normalize_kernel<<>>(N, x, mean, variance, batch, filters, spatial); +- yield_gpu_with_remark("normalize_kernel"); ++ normalize_kernel<<>>(N, x, mean, variance, batch, filters, spatial); + + check_error(cudaPeekAtLastError()); + } +@@ -528,9 +506,7 @@ extern "C" void l2normalize_gpu(float *x, float *dx, int batch, int filters, int + { + size_t N = batch*spatial; + +- request_gpu(); +- l2norm_kernel<<>>(N, x, dx, batch, filters, spatial); +- yield_gpu_with_remark("l2norm_kernel"); ++ l2norm_kernel<<>>(N, x, dx, batch, filters, spatial); + + check_error(cudaPeekAtLastError()); + } +@@ -596,18 +572,14 @@ __global__ void fast_variance_kernel(float *x, float *mean, int batch, int filt + + extern "C" void fast_mean_gpu(float *x, int batch, int filters, int spatial, float *mean) + { +- request_gpu(); +- fast_mean_kernel<<>>(x, batch, filters, spatial, mean); +- yield_gpu_with_remark("fast_mean_kernel"); ++ fast_mean_kernel<<>>(x, batch, filters, spatial, mean); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void fast_variance_gpu(float *x, float *mean, int batch, int filters, int spatial, float *variance) + { +- request_gpu(); +- fast_variance_kernel<<>>(x, mean, batch, filters, spatial, variance); +- yield_gpu_with_remark("fast_mean_kernel"); ++ fast_variance_kernel<<>>(x, mean, batch, filters, spatial, variance); + + check_error(cudaPeekAtLastError()); + } +@@ -615,18 +587,14 @@ extern "C" void fast_variance_gpu(float *x, float *mean, int batch, int filters, + + extern "C" void mean_gpu(float *x, int batch, int filters, int spatial, float *mean) + { +- request_gpu(); +- mean_kernel<<>>(x, batch, filters, spatial, mean); +- yield_gpu_with_remark("mean_kernel"); ++ mean_kernel<<>>(x, batch, filters, spatial, mean); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void variance_gpu(float *x, float *mean, int batch, int filters, int spatial, float *variance) + { +- request_gpu(); +- variance_kernel<<>>(x, mean, batch, filters, spatial, variance); +- yield_gpu_with_remark("variance_kernel"); ++ variance_kernel<<>>(x, mean, batch, filters, spatial, variance); + + check_error(cudaPeekAtLastError()); + } +@@ -638,18 +606,14 @@ extern "C" void axpy_gpu(int N, float ALPHA, float * X, int INCX, float * Y, int + + extern "C" void pow_gpu(int N, float ALPHA, float * X, int INCX, float * Y, int INCY) + { +- request_gpu(); +- pow_kernel<<>>(N, ALPHA, X, INCX, Y, INCY); +- yield_gpu_with_remark("pow_kernel"); ++ pow_kernel<<>>(N, ALPHA, X, INCX, Y, INCY); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void axpy_gpu_offset(int N, float ALPHA, float * X, int OFFX, int INCX, float * Y, int OFFY, int INCY) + { +- request_gpu(); +- axpy_kernel<<>>(N, ALPHA, X, OFFX, INCX, Y, OFFY, INCY); +- yield_gpu_with_remark("pow_kernel"); ++ axpy_kernel<<>>(N, ALPHA, X, OFFX, INCX, Y, OFFY, INCY); + + check_error(cudaPeekAtLastError()); + } +@@ -661,18 +625,14 @@ extern "C" void copy_gpu(int N, float * X, int INCX, float * Y, int INCY) + + extern "C" void mul_gpu(int N, float * X, int INCX, float * Y, int INCY) + { +- request_gpu(); +- mul_kernel<<>>(N, X, INCX, Y, INCY); +- yield_gpu_with_remark("mul_kernel"); ++ mul_kernel<<>>(N, X, INCX, Y, INCY); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void copy_gpu_offset(int N, float * X, int OFFX, int INCX, float * Y, int OFFY, int INCY) + { +- request_gpu(); +- copy_kernel<<>>(N, X, OFFX, INCX, Y, OFFY, INCY); +- yield_gpu_with_remark("copy_kernel"); ++ copy_kernel<<>>(N, X, OFFX, INCX, Y, OFFY, INCY); + + check_error(cudaPeekAtLastError()); + } +@@ -698,9 +658,7 @@ extern "C" void flatten_gpu(float *x, int spatial, int layers, int batch, int fo + { + int size = spatial*batch*layers; + +- request_gpu(); +- flatten_kernel<<>>(size, x, spatial, layers, batch, forward, out); +- yield_gpu_with_remark("flatten_kernel"); ++ flatten_kernel<<>>(size, x, spatial, layers, batch, forward, out); + + check_error(cudaPeekAtLastError()); + } +@@ -709,9 +667,7 @@ extern "C" void reorg_gpu(float *x, int w, int h, int c, int batch, int stride, + { + int size = w*h*c*batch; + +- request_gpu(); +- reorg_kernel<<>>(size, x, w, h, c, batch, stride, forward, out); +- yield_gpu_with_remark("reorg_kernel"); ++ reorg_kernel<<>>(size, x, w, h, c, batch, stride, forward, out); + + check_error(cudaPeekAtLastError()); + } +@@ -724,9 +680,7 @@ __global__ void mask_kernel(int n, float *x, float mask_num, float *mask, float + + extern "C" void mask_gpu(int N, float * X, float mask_num, float * mask, float val) + { +- request_gpu(); +- mask_kernel<<>>(N, X, mask_num, mask, val); +- yield_gpu_with_remark("mask_kernel"); ++ mask_kernel<<>>(N, X, mask_num, mask, val); + + check_error(cudaPeekAtLastError()); + } +@@ -739,27 +693,21 @@ __global__ void scale_mask_kernel(int n, float *x, float mask_num, float *mask, + + extern "C" void scale_mask_gpu(int N, float * X, float mask_num, float * mask, float scale) + { +- request_gpu(); +- scale_mask_kernel<<>>(N, X, mask_num, mask, scale); +- yield_gpu_with_remark("scale_mask_kernel"); ++ scale_mask_kernel<<>>(N, X, mask_num, mask, scale); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void const_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- const_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("const_kernel"); ++ const_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void constrain_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- constrain_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("constrain_kernel"); ++ constrain_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } +@@ -767,36 +715,28 @@ extern "C" void constrain_gpu(int N, float ALPHA, float * X, int INCX) + + extern "C" void add_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- add_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("add_kernel"); ++ add_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void scal_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- scal_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("scal_kernel"); ++ scal_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void supp_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- supp_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("supp_kernel"); ++ supp_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void fill_gpu(int N, float ALPHA, float * X, int INCX) + { +- request_gpu(); +- fill_kernel<<>>(N, ALPHA, X, INCX); +- yield_gpu_with_remark("fill_kernel"); ++ fill_kernel<<>>(N, ALPHA, X, INCX); + + check_error(cudaPeekAtLastError()); + } +@@ -834,9 +774,7 @@ extern "C" void shortcut_gpu(int batch, int w1, int h1, int c1, float *add, int + + int size = batch * minw * minh * minc; + +- request_gpu(); +- shortcut_kernel<<>>(size, minw, minh, minc, stride, sample, batch, w1, h1, c1, add, w2, h2, c2, s1, s2, out); +- yield_gpu_with_remark("shortcut_kernel"); ++ shortcut_kernel<<>>(size, minw, minh, minc, stride, sample, batch, w1, h1, c1, add, w2, h2, c2, s1, s2, out); + + check_error(cudaPeekAtLastError()); + } +@@ -860,9 +798,7 @@ __global__ void smooth_l1_kernel(int n, float *pred, float *truth, float *delta, + + extern "C" void smooth_l1_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- smooth_l1_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("smooth_l1_kernel"); ++ smooth_l1_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -880,9 +816,7 @@ __global__ void softmax_x_ent_kernel(int n, float *pred, float *truth, float *de + + extern "C" void softmax_x_ent_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- softmax_x_ent_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("softmax_x_ent_kernel"); ++ softmax_x_ent_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -900,9 +834,7 @@ __global__ void logistic_x_ent_kernel(int n, float *pred, float *truth, float *d + + extern "C" void logistic_x_ent_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- logistic_x_ent_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("logistic_x_ent_kernel"); ++ logistic_x_ent_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -919,9 +851,7 @@ __global__ void l2_kernel(int n, float *pred, float *truth, float *delta, float + + extern "C" void l2_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- l2_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("l2_kernel"); ++ l2_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -938,9 +868,7 @@ __global__ void l1_kernel(int n, float *pred, float *truth, float *delta, float + + extern "C" void l1_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- l1_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("l1_kernel"); ++ l1_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -956,9 +884,7 @@ __global__ void wgan_kernel(int n, float *pred, float *truth, float *delta, floa + + extern "C" void wgan_gpu(int n, float *pred, float *truth, float *delta, float *error) + { +- request_gpu(); +- wgan_kernel<<>>(n, pred, truth, delta, error); +- yield_gpu_with_remark("wgan_kernel"); ++ wgan_kernel<<>>(n, pred, truth, delta, error); + + check_error(cudaPeekAtLastError()); + } +@@ -990,9 +916,7 @@ __global__ void deinter_kernel(int NX, float *X, int NY, float *Y, int B, float + + extern "C" void deinter_gpu(int NX, float *X, int NY, float *Y, int B, float *OUT) + { +- request_gpu(); +- deinter_kernel<<>>(NX, X, NY, Y, B, OUT); +- yield_gpu_with_remark("deinter_kernel"); ++ deinter_kernel<<>>(NX, X, NY, Y, B, OUT); + + check_error(cudaPeekAtLastError()); + } +@@ -1013,18 +937,14 @@ __global__ void inter_kernel(int NX, float *X, int NY, float *Y, int B, float *O + + extern "C" void inter_gpu(int NX, float *X, int NY, float *Y, int B, float *OUT) + { +- request_gpu(); +- inter_kernel<<>>(NX, X, NY, Y, B, OUT); +- yield_gpu_with_remark("inter_kernel"); ++ inter_kernel<<>>(NX, X, NY, Y, B, OUT); + + check_error(cudaPeekAtLastError()); + } + + extern "C" void weighted_sum_gpu(float *a, float *b, float *s, int num, float *c) + { +- request_gpu(); +- weighted_sum_kernel<<>>(num, a, b, s, c); +- yield_gpu_with_remark("weighted_sum_kernel"); ++ weighted_sum_kernel<<>>(num, a, b, s, c); + + check_error(cudaPeekAtLastError()); + } +@@ -1041,9 +961,7 @@ __global__ void weighted_delta_kernel(int n, float *a, float *b, float *s, float + + extern "C" void weighted_delta_gpu(float *a, float *b, float *s, float *da, float *db, float *ds, int num, float *dc) + { +- request_gpu(); +- weighted_delta_kernel<<>>(num, a, b, s, da, db, ds, dc); +- yield_gpu_with_remark("weighted_delta_kernel"); ++ weighted_delta_kernel<<>>(num, a, b, s, da, db, ds, dc); + + check_error(cudaPeekAtLastError()); + } +@@ -1058,9 +976,7 @@ __global__ void mult_add_into_kernel(int n, float *a, float *b, float *c) + + extern "C" void mult_add_into_gpu(int num, float *a, float *b, float *c) + { +- request_gpu(); +- mult_add_into_kernel<<>>(num, a, b, c); +- yield_gpu_with_remark("mult_add_into_kernel"); ++ mult_add_into_kernel<<>>(num, a, b, c); + + check_error(cudaPeekAtLastError()); + } +@@ -1113,9 +1029,7 @@ extern "C" void softmax_tree(float *input, int spatial, int batch, int stride, f + */ + int num = spatial*batch*hier.groups; + +- request_gpu(); +- softmax_tree_kernel<<>>(input, spatial, batch, stride, temp, output, hier.groups, tree_groups_size, tree_groups_offset); +- yield_gpu_with_remark("softmax_tree_kernel"); ++ softmax_tree_kernel<<>>(input, spatial, batch, stride, temp, output, hier.groups, tree_groups_size, tree_groups_offset); + + check_error(cudaPeekAtLastError()); + cuda_free((float *)tree_groups_size); +@@ -1133,9 +1047,7 @@ __global__ void softmax_kernel(float *input, int n, int batch, int batch_offset, + + extern "C" void softmax_gpu(float *input, int n, int batch, int batch_offset, int groups, int group_offset, int stride, float temp, float *output) + { +- request_gpu(); +- softmax_kernel<<>>(input, n, batch, batch_offset, groups, group_offset, stride, temp, output); +- yield_gpu_with_remark("softmax_tree_kernel"); ++ softmax_kernel<<>>(input, n, batch, batch_offset, groups, group_offset, stride, temp, output); + + check_error(cudaPeekAtLastError()); + } +@@ -1168,9 +1080,7 @@ extern "C" void upsample_gpu(float *in, int w, int h, int c, int batch, int stri + { + size_t size = w*h*c*batch*stride*stride; + +- request_gpu(); +- upsample_kernel<<>>(size, in, w, h, c, batch, stride, forward, scale, out); +- yield_gpu_with_remark("upsample_kernel"); ++ upsample_kernel<<>>(size, in, w, h, c, batch, stride, forward, scale, out); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu +index ad00edde..64a35528 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/col2im_kernels.cu +@@ -50,12 +50,10 @@ void col2im_gpu(float *data_col, + int width_col = (width + 2 * pad - ksize) / stride + 1; + int num_kernels = channels * height * width; + +- request_gpu(); +- col2im_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK, ++ col2im_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK, + BLOCK>>>( + num_kernels, data_col, height, width, ksize, pad, + stride, height_col, + width_col, data_im); +- yield_gpu_with_remark("col2im_gpu_kernel"); + } + +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu +index 7792dc81..95f07477 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/convolutional_kernels.cu +@@ -22,9 +22,7 @@ __global__ void binarize_kernel(float *x, int n, float *binary) + + void binarize_gpu(float *x, int n, float *binary) + { +- request_gpu(); +- binarize_kernel<<>>(x, n, binary); +- yield_gpu_with_remark("binarize_kernel"); ++ binarize_kernel<<>>(x, n, binary); + + check_error(cudaPeekAtLastError()); + } +@@ -46,9 +44,7 @@ __global__ void binarize_input_kernel(float *input, int n, int size, float *bina + + void binarize_input_gpu(float *input, int n, int size, float *binary) + { +- request_gpu(); +- binarize_input_kernel<<>>(input, n, size, binary); +- yield_gpu_with_remark("binarize_input_kernel"); ++ binarize_input_kernel<<>>(input, n, size, binary); + + check_error(cudaPeekAtLastError()); + } +@@ -72,9 +68,7 @@ __global__ void binarize_weights_kernel(float *weights, int n, int size, float * + + void binarize_weights_gpu(float *weights, int n, int size, float *binary) + { +- request_gpu(); +- binarize_weights_kernel<<>>(weights, n, size, binary); +- yield_gpu_with_remark("binarize_weights_kernel"); ++ binarize_weights_kernel<<>>(weights, n, size, binary); + + check_error(cudaPeekAtLastError()); + } +@@ -177,9 +171,7 @@ extern "C" void smooth_layer(layer l, int size, float rate) + + size_t n = h*w*c*l.batch; + +- request_gpu(); +- smooth_kernel<<>>(l.output_gpu, n, l.w, l.h, l.c, size, rate, l.delta_gpu); +- yield_gpu_with_remark("smooth_kernel"); ++ smooth_kernel<<>>(l.output_gpu, n, l.w, l.h, l.c, size, rate, l.delta_gpu); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu +index d75e257d..539cfdba 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/crop_layer_kernels.cu +@@ -195,17 +195,13 @@ extern "C" void forward_crop_layer_gpu(crop_layer layer, network net) + + int size = layer.batch * layer.w * layer.h; + +- request_gpu(); +- levels_image_kernel<<>>(net.input_gpu, layer.rand_gpu, layer.batch, layer.w, layer.h, net.train, layer.saturation, layer.exposure, translate, scale, layer.shift); +- yield_gpu_with_remark("levels_image_kernel"); ++ levels_image_kernel<<>>(net.input_gpu, layer.rand_gpu, layer.batch, layer.w, layer.h, net.train, layer.saturation, layer.exposure, translate, scale, layer.shift); + + check_error(cudaPeekAtLastError()); + + size = layer.batch*layer.c*layer.out_w*layer.out_h; + +- request_gpu(); +- forward_crop_layer_kernel<<>>(net.input_gpu, layer.rand_gpu, size, layer.c, layer.h, layer.w, layer.out_h, layer.out_w, net.train, layer.flip, radians, layer.output_gpu); +- yield_gpu_with_remark("forward_crop_layer_kernel"); ++ forward_crop_layer_kernel<<>>(net.input_gpu, layer.rand_gpu, size, layer.c, layer.h, layer.w, layer.out_h, layer.out_w, net.train, layer.flip, radians, layer.output_gpu); + + check_error(cudaPeekAtLastError()); + +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c +index a9da26f1..5c1bfc0d 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/cuda.c +@@ -92,15 +92,11 @@ float *cuda_make_array(float *x, size_t n) + float *x_gpu; + size_t size = sizeof(float)*n; + +- request_gpu(); +- cudaError_t status = cudaMalloc((void **)&x_gpu, size); +- yield_gpu_with_remark("cudaMalloc"); ++ cudaError_t status = cudaMalloc((void **)&x_gpu, size); + + check_error(status); + if(x){ +- request_gpu(); +- status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); +- yield_gpu_with_remark("cuda_make_array"); ++ status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); + check_error(status); + } else { + fill_gpu(n, 0, x_gpu, 1); +@@ -114,25 +110,17 @@ void cuda_random(float *x_gpu, size_t n) + static curandGenerator_t gen[16]; + static int init[16] = {0}; + +- request_gpu(); +- int i = cuda_get_device(); +- yield_gpu_with_remark("cuda_get_device"); ++ int i = cuda_get_device(); + + if(!init[i]){ +- request_gpu(); +- curandCreateGenerator(&gen[i], CURAND_RNG_PSEUDO_DEFAULT); +- yield_gpu_with_remark("curandCreateGenerator"); ++ curandCreateGenerator(&gen[i], CURAND_RNG_PSEUDO_DEFAULT); + +- request_gpu(); +- curandSetPseudoRandomGeneratorSeed(gen[i], time(0)); +- yield_gpu_with_remark("curandSetPseudoRandomGeneratorSeed"); ++ curandSetPseudoRandomGeneratorSeed(gen[i], time(0)); + + init[i] = 1; + } + +- request_gpu(); +- curandGenerateUniform(gen[i], x_gpu, n); +- yield_gpu_with_remark("curandGenerateUniform"); ++ curandGenerateUniform(gen[i], x_gpu, n); + + check_error(cudaPeekAtLastError()); + } +@@ -155,15 +143,11 @@ int *cuda_make_int_array(int *x, size_t n) + int *x_gpu; + size_t size = sizeof(int)*n; + +- request_gpu(); +- cudaError_t status = cudaMalloc((void **)&x_gpu, size); +- yield_gpu_with_remark("cudaMalloc"); ++ cudaError_t status = cudaMalloc((void **)&x_gpu, size); + + check_error(status); + if(x){ +- request_gpu(); +- status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); +- yield_gpu_with_remark("cuda_make_int_array"); ++ status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); + + check_error(status); + } +@@ -173,9 +157,7 @@ int *cuda_make_int_array(int *x, size_t n) + + void cuda_free(float *x_gpu) + { +- request_gpu(); +- cudaError_t status = cudaFree(x_gpu); +- yield_gpu_with_remark("free"); ++ cudaError_t status = cudaFree(x_gpu); + + check_error(status); + } +@@ -183,9 +165,7 @@ void cuda_free(float *x_gpu) + void cuda_push_array(float *x_gpu, float *x, size_t n) + { + size_t size = sizeof(float)*n; +- request_gpu(); +- cudaError_t status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); +- yield_gpu_with_remark("cuda_push_array"); ++ cudaError_t status = cudaMemcpy(x_gpu, x, size, cudaMemcpyHostToDevice); + + check_error(status); + } +@@ -194,9 +174,7 @@ void cuda_pull_array(float *x_gpu, float *x, size_t n) + { + size_t size = sizeof(float)*n; + +- request_gpu(); +- cudaError_t status = cudaMemcpy(x, x_gpu, size, cudaMemcpyDeviceToHost); +- yield_gpu_with_remark("cuda_pull_array"); ++ cudaError_t status = cudaMemcpy(x, x_gpu, size, cudaMemcpyDeviceToHost); + + check_error(status); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu +index ad24b679..c6774fa7 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/dropout_layer_kernels.cu +@@ -27,9 +27,7 @@ void forward_dropout_layer_gpu(dropout_layer layer, network net) + cuda_push_array(layer.rand_gpu, layer.rand, size); + */ + +- request_gpu(); +- yoloswag420blazeit360noscope<<>>(net.input_gpu, size, layer.rand_gpu, layer.probability, layer.scale); +- yield_gpu_with_remark("yoloswag420blazeit360noscope_forward"); ++ yoloswag420blazeit360noscope<<>>(net.input_gpu, size, layer.rand_gpu, layer.probability, layer.scale); + + check_error(cudaPeekAtLastError()); + } +@@ -39,9 +37,7 @@ void backward_dropout_layer_gpu(dropout_layer layer, network net) + if(!net.delta_gpu) return; + int size = layer.inputs*layer.batch; + +- request_gpu(); +- yoloswag420blazeit360noscope<<>>(net.delta_gpu, size, layer.rand_gpu, layer.probability, layer.scale); +- yield_gpu_with_remark("yoloswag420blazeit360noscope_backward"); ++ yoloswag420blazeit360noscope<<>>(net.delta_gpu, size, layer.rand_gpu, layer.probability, layer.scale); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu +index 0a73ec88..d2ccbdee 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/im2col_kernels.cu +@@ -54,11 +54,9 @@ void im2col_gpu(float *im, + int width_col = (width + 2 * pad - ksize) / stride + 1; + int num_kernels = channels * height_col * width_col; + +- request_gpu(); +- im2col_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK, ++ im2col_gpu_kernel<<<(num_kernels+BLOCK-1)/BLOCK, + BLOCK>>>( + num_kernels, im, height, width, ksize, pad, + stride, height_col, + width_col, data_col); +- yield_gpu_with_remark("im2col_gpu_kernel"); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu +index 285ae322..12d920ee 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/darknet/src/maxpool_layer_kernels.cu +@@ -92,9 +92,7 @@ extern "C" void forward_maxpool_layer_gpu(maxpool_layer layer, network net) + + size_t n = h*w*c*layer.batch; + +- request_gpu(); +- forward_maxpool_layer_kernel<<>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, net.input_gpu, layer.output_gpu, layer.indexes_gpu); +- yield_gpu_with_remark("im2col_gpu_kernel"); ++ forward_maxpool_layer_kernel<<>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, net.input_gpu, layer.output_gpu, layer.indexes_gpu); + + check_error(cudaPeekAtLastError()); + } +@@ -103,9 +101,7 @@ extern "C" void backward_maxpool_layer_gpu(maxpool_layer layer, network net) + { + size_t n = layer.h*layer.w*layer.c*layer.batch; + +- request_gpu(); +- backward_maxpool_layer_kernel<<>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, layer.delta_gpu, net.delta_gpu, layer.indexes_gpu); +- yield_gpu_with_remark("im2col_gpu_kernel"); ++ backward_maxpool_layer_kernel<<>>(n, layer.h, layer.w, layer.c, layer.stride, layer.size, layer.pad, layer.delta_gpu, net.delta_gpu, layer.indexes_gpu); + + check_error(cudaPeekAtLastError()); + } +diff --git a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp +index 133eaf33..7bc19835 100644 +--- a/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp ++++ b/autoware.ai/src/autoware/core_perception/vision_darknet_detect/src/vision_darknet_detect.cpp +@@ -282,12 +282,6 @@ void Yolo3DetectorNode::image_callback(const sensor_msgs::ImageConstPtr& in_imag + + free(darknet_image_.data); + +- if(is_task_ready_ == TASK_NOT_READY){ +- init_task(); +- if(gpu_profiling_flag_) start_gpu_profiling(); +- } +- +- task_state_ = TASK_STATE_DONE; + } + + void Yolo3DetectorNode::config_cb(const autoware_config_msgs::ConfigSSD::ConstPtr& param) +@@ -327,52 +321,22 @@ void Yolo3DetectorNode::Run() + int key_id = 2; + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename_str; + int rate; + double task_minimum_inter_release_time; + double task_execution_time; + double task_relative_deadline; + +- int gpu_scheduling_flag; +- int gpu_profiling_flag; +- std::string gpu_execution_time_filename_str; +- std::string gpu_response_time_filename_str; +- std::string gpu_deadline_filename_str; +- +- private_node_handle.param("/vision_darknet_detect/task_scheduling_flag", task_scheduling_flag, 0); +- private_node_handle.param("/vision_darknet_detect/task_profiling_flag", task_profiling_flag, 0); + private_node_handle.param("/vision_darknet_detect/task_response_time_filename", task_response_time_filename_str, "~/Documents/profiling/response_time/vision_darknet_detect.csv"); + private_node_handle.param("/vision_darknet_detect/rate", rate, 10); + private_node_handle.param("/vision_darknet_detect/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + private_node_handle.param("/vision_darknet_detect/task_execution_time", task_execution_time, (double)10); + private_node_handle.param("/vision_darknet_detect/task_relative_deadline", task_relative_deadline, (double)10); +- private_node_handle.param("/vision_darknet_detect/gpu_scheduling_flag", gpu_scheduling_flag, 0); +- private_node_handle.param("/vision_darknet_detect/gpu_profiling_flag", gpu_profiling_flag, 0); +- private_node_handle.param("/vision_darknet_detect/gpu_execution_time_filename", gpu_execution_time_filename_str, "~/Documents/gpu_profiling/test_yolo_execution_time.csv"); +- private_node_handle.param("/vision_darknet_detect/gpu_response_time_filename", gpu_response_time_filename_str, "~/Documents/gpu_profiling/test_yolo_response_time.csv"); +- private_node_handle.param("/vision_darknet_detect/gpu_deadline_filename", gpu_deadline_filename_str, "~/Documents/gpu_deadline/yolo_gpu_deadline.csv"); +- +- +- + + char* task_response_time_filename = strdup(task_response_time_filename_str.c_str()); +- char* gpu_execution_time_filename = strdup(gpu_execution_time_filename_str.c_str()); +- char* gpu_response_time_filename = strdup(gpu_response_time_filename_str.c_str()); +- char* gpu_deadline_filename = strdup(gpu_deadline_filename_str.c_str()); +- +- if(task_profiling_flag) init_task_profiling(task_response_time_filename); +- if(gpu_profiling_flag) init_gpu_profiling(gpu_execution_time_filename, gpu_response_time_filename); +- +- if(gpu_scheduling_flag){ +- init_gpu_scheduling("/tmp/yolo", gpu_deadline_filename, key_id); +- }else if(gpu_scheduling_flag){ +- ROS_ERROR("GPU scheduling flag is true but type doesn't set to GPU!"); +- exit(1); +- } +- +- ++ ++ init_task_profiling(task_response_time_filename); ++ + //RECEIVE IMAGE TOPIC NAME + std::string image_raw_topic_str; + if (private_node_handle.getParam("image_raw_node", image_raw_topic_str)) +@@ -449,39 +413,17 @@ void Yolo3DetectorNode::Run() + + ROS_INFO_STREAM( __APP_NAME__ << "" ); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(is_task_ready_) break; +- ros::spinOnce(); +- r.sleep(); +- } ++ ros::Rate r(rate); + +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) start_task_profiling(); +- if(task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- if(gpu_profiling_flag || gpu_scheduling_flag) start_job(); +- task_state_ = TASK_STATE_RUNNING; +- } ++ // Executing task ++ while(ros::ok()){ ++ start_task_profiling(); + +- ros::spinOnce(); ++ ros::spinOnce(); + +- if(task_profiling_flag) stop_task_profiling(0, task_state_); ++ stop_task_profiling(0, 0); + +- if(task_state_ == TASK_STATE_DONE){ +- if(gpu_profiling_flag || gpu_scheduling_flag) finish_job(); +- if(task_scheduling_flag) yield_task_scheduling(); +- task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ r.sleep(); + } + ROS_INFO("END Yolo"); + +diff --git a/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp b/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp +index 67a65d23..9517d002 100644 +--- a/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_global_planner/nodes/op_global_planner_core.cpp +@@ -240,7 +240,6 @@ void GlobalPlanner::callbackGetVehicleStatus(const geometry_msgs::TwistStampedCo + m_VehicleState.steer = atan(2.7 * msg->twist.angular.z/msg->twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleState.tStamp); + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); + } + + void GlobalPlanner::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) +@@ -447,8 +446,6 @@ void GlobalPlanner::MainLoop() + ros::NodeHandle private_nh("~"); + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; +@@ -458,8 +455,6 @@ void GlobalPlanner::MainLoop() + double multilap_replanning_distance; + int planning_fail_cnt; + +- private_nh.param("/op_global_planner/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_global_planner/task_profiling_flag", task_profiling_flag, 0); + private_nh.param("/op_global_planner/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_global_planner.csv"); + private_nh.param("/op_global_planner/rate", rate, 10); + private_nh.param("/op_global_planner/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +@@ -468,24 +463,16 @@ void GlobalPlanner::MainLoop() + private_nh.param("/op_global_planner/multilap_flag", multilap_flag, 0); + private_nh.param("/op_global_planner/multilap_replanning_distance", multilap_replanning_distance, (double)50); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + timespec animation_timer; + UtilityHNS::UtilityH::GetTickCount(animation_timer); + + ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25); +- +- + + while (ros::ok()) + { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ rubis::start_task_profiling(); + + ros::spinOnce(); + bool bMakeNewPlan = false; +@@ -618,14 +605,10 @@ void GlobalPlanner::MainLoop() + VisualizeDestinations(m_GoalsPos, m_iCurrentGoalIndex); + } + +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); ++ rubis::stop_task_profiling(0, 0); + +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } + loop_rate.sleep(); + } + } +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h +index 708301ff..ee2efa6d 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_behavior_selector_core.h +@@ -61,12 +61,17 @@ + #include + #include + ++#include "rubis_msgs/LaneArrayWithPoseTwist.h" ++#include "rubis_msgs/LaneWithPoseTwist.h" ++ + + namespace BehaviorGeneratorNS + { + + class BehaviorGen + { ++public: ++ const rubis_msgs::LaneArrayWithPoseTwist lane_array_with_pose_twist_msg_; + protected: //Planning Related variables + double PI = 3.14159265; + +@@ -120,12 +125,14 @@ protected: //Planning Related variables + double m_sprintSpeed; + bool m_sprintSwitch; + double m_obstacleWaitingTimeinIntersection; ++ double distance_to_pdestrian_; + + //ROS messages (topics) + ros::NodeHandle nh; + + //define publishers + ros::Publisher pub_LocalPath; ++ ros::Publisher pub_LocalPathWithPosePub; + ros::Publisher pub_LocalBasePath; + ros::Publisher pub_ClosestIndex; + ros::Publisher pub_BehaviorState; +@@ -159,15 +166,18 @@ protected: //Planning Related variables + ros::Subscriber sub_SprintSwitch; + ros::Subscriber sub_IntersectionCondition; + ++ // Others ++ timespec planningTimer; ++ std_msgs::Bool emergency_stop_msg; ++ + // Callback function for subscriber. +- void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); +- void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg); + void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); + void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg); + void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); +- void callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); ++ void callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg); + void callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstPtr& msg); + void callbackDistanceToPedestrian(const std_msgs::Float64& msg); ++ void _callbackDistanceToPedestrian(); + void callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg); + + void callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg); +@@ -179,7 +189,7 @@ protected: //Planning Related variables + + //Helper Functions + void UpdatePlanningParams(ros::NodeHandle& _nh); +- void SendLocalPlanningTopics(); ++ void SendLocalPlanningTopics(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg); + void VisualizeLocalPlanner(); + void LogLocalPlanningInfo(double dt); + bool GetBaseMapTF(); +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h +index 3890c27d..d1bf4bb6 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_motion_predictor_core.h +@@ -125,9 +125,12 @@ protected: + ros::Subscriber sub_can_info ; + ros::Subscriber sub_StepSignal; + ++ rubis_msgs::DetectedObjectArray objects_msgs_; ++ + // Callback function for subscriber. + void callbackGetTrackedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg); + void callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg); ++ void _callbackGetRubisTrackedObjects(rubis_msgs::DetectedObjectArray& objects_msg); + void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); + void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg); + void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h +index f2c4576d..a0324e44 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_evaluator_core.h +@@ -37,6 +37,7 @@ + #include + #include + #include ++#include "rubis_msgs/LaneArrayWithPoseTwist.h" + + #include "op_planner/PlannerCommonDef.h" + #include "op_planner/TrajectoryDynamicCosts.h" +@@ -50,6 +51,7 @@ namespace TrajectoryEvaluatorNS + class TrajectoryEval + { + protected: ++ bool is_objects_updated_; + + PlannerHNS::TrajectoryDynamicCosts m_TrajectoryCostsCalculator; + bool m_bUseMoveingObjectsPrediction; +@@ -101,6 +103,7 @@ protected: + ros::Publisher pub_CollisionPointsRviz; + ros::Publisher pub_LocalWeightedTrajectoriesRviz; + ros::Publisher pub_LocalWeightedTrajectories; ++ ros::Publisher pub_LocalWeightedTrajectoriesWithPoseTwist; + ros::Publisher pub_TrajectoryCost; + ros::Publisher pub_SafetyBorderRviz; + ros::Publisher pub_DistanceToPedestrian; +@@ -128,6 +131,9 @@ protected: + tf::StampedTransform m_velodyne_to_base_link; + tf::StampedTransform m_velodyne_to_map; + ++ // Others ++ std::vector intersection_list_; ++ autoware_msgs::DetectedObjectArray object_msg_; + + // Callback function for subscriber. + void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); +@@ -135,8 +141,9 @@ protected: + void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); + void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg); + void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); +- void callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); +- void callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg); ++ void callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg); ++ void callbackGetPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg); ++ void _callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArray& objects); + void callbackGetRubisPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg); + void callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr & msg); + void callbackGetCurrentState(const std_msgs::Int32 & msg); +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h +index f7dddce1..c47e5c2e 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/include/op_trajectory_generator_core.h +@@ -30,12 +30,18 @@ + #include "op_planner/PlannerH.h" + #include "op_planner/PlannerCommonDef.h" + ++#include "rubis_msgs/PoseTwistStamped.h" ++#include "rubis_msgs/LaneArrayWithPoseTwist.h" ++ + namespace TrajectoryGeneratorNS + { + + class TrajectoryGen + { + protected: ++ geometry_msgs::PoseStamped current_pose_; ++ geometry_msgs::TwistStamped current_twist_; ++ + PlannerHNS::PlannerH m_Planner; + geometry_msgs::Pose m_OriginPos; + PlannerHNS::WayPoint m_InitPos; +@@ -59,11 +65,12 @@ protected: + PlannerHNS::CAR_BASIC_INFO m_CarInfo; + + +- //ROS messages (topics) ++ //ROS messages (topics) + ros::NodeHandle nh; + + //define publishers + ros::Publisher pub_LocalTrajectories; ++ ros::Publisher pub_LocalTrajectoriesWithPoseTwist; + ros::Publisher pub_LocalTrajectoriesRviz; + + // define subscribers. +@@ -73,16 +80,22 @@ protected: + ros::Subscriber sub_robot_odom; + ros::Subscriber sub_can_info; + ros::Subscriber sub_GlobalPlannerPaths; ++ ros::Subscriber sub_pose_twist; + ++ // Others ++ + + // Callback function for subscriber. + void callbackGetInitPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &input); +- void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); ++ // void callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); ++ ++ void callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg); + void callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg); + void callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg); + void callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg); + void callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg); + ++ + //Helper Functions + void UpdatePlanningParams(ros::NodeHandle& _nh); + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch +index 0e794331..69cb1a60 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector.launch +@@ -12,7 +12,7 @@ + + + +- ++ + + + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch +index 7899250e..7c0bf9ac 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_behavior_selector_parameter.launch +@@ -8,7 +8,7 @@ + + + +- ++ + + + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch +index 37026b00..ef56b671 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator.launch +@@ -1,6 +1,8 @@ + + + ++ ++ + + + +@@ -11,6 +13,10 @@ + + + ++ ++ ++ ++ + + + +@@ -25,7 +31,8 @@ + + + +- ++ ++ + + + +@@ -34,7 +41,11 @@ + + + +- ++ ++ ++ ++ ++ + + + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch +index 5b4c21ac..c1a16cfc 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/launch/op_trajectory_evaluator_parameter.launch +@@ -1,10 +1,15 @@ + + ++ + + + + + ++ ++ ++ ++ + + + +@@ -12,11 +17,17 @@ + + + ++ ++ + + + + +- ++ ++ ++ ++ ++ + + + +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp +index 9d824cfd..3c22305d 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.cpp +@@ -34,6 +34,8 @@ BehaviorGen::BehaviorGen() + bBestCost = false; + bMap = false; + bRollOuts = false; ++ UtilityHNS::UtilityH::GetTickCount(planningTimer); ++ distance_to_pdestrian_ = 1000.0; + + ros::NodeHandle _nh; + UpdatePlanningParams(_nh); +@@ -50,6 +52,7 @@ BehaviorGen::BehaviorGen() + m_OriginPos.position.z = transform.getOrigin().z(); + + pub_LocalPath = nh.advertise("final_waypoints", 1,true); ++ pub_LocalPathWithPosePub = nh.advertise("final_waypoints_with_pose_twist", 1,true); + pub_LocalBasePath = nh.advertise("base_waypoints", 1,true); + pub_ClosestIndex = nh.advertise("closest_waypoint", 1,true); + pub_BehaviorState = nh.advertise("current_behavior", 1); +@@ -61,36 +64,19 @@ BehaviorGen::BehaviorGen() + pub_turnMarker = nh.advertise("turn_marker", 1); + pub_currentState = nh.advertise("current_state", 1); + +- sub_current_pose = nh.subscribe("/current_pose", 10, &BehaviorGen::callbackGetCurrentPose, this); +- +- int bVelSource = 1; +- _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); +- if(bVelSource == 0) +- sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this); +- else if(bVelSource == 1) +- sub_current_velocity = nh.subscribe("/current_velocity", 10, &BehaviorGen::callbackGetVehicleStatus, this); +- else if(bVelSource == 2) +- sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this); +- +- /* RT Scheduling setup */ +- // sub_current_pose = nh.subscribe("/current_pose", 1, &BehaviorGen::callbackGetCurrentPose, this); //origin 10 +- + // int bVelSource = 1; + // _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); + // if(bVelSource == 0) +- // sub_robot_odom = nh.subscribe("/odom", 1, &BehaviorGen::callbackGetRobotOdom, this); //origin 10 +- // else if(bVelSource == 1) +- // sub_current_velocity = nh.subscribe("/current_velocity", 1, &BehaviorGen::callbackGetVehicleStatus, this); //origin 10 ++ // sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this); + // else if(bVelSource == 2) +- // sub_can_info = nh.subscribe("/can_info", 1, &BehaviorGen::callbackGetCANInfo, this); //origin 10 ++ // sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this); + + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); +- sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories", 1, &BehaviorGen::callbackGetLocalPlannerPath, this); ++ sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories_with_pose_twist", 1, &BehaviorGen::callbackGetLocalPlannerPath, this); + // sub_TrafficLightStatus = nh.subscribe("/light_color", 1, &BehaviorGen::callbackGetTrafficLightStatus, this); + // sub_TrafficLightSignals = nh.subscribe("/roi_signal", 1, &BehaviorGen::callbackGetTrafficLightSignals, this); + sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this); +- +- sub_TrafficLightSignals = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this); ++ // sub_TrafficLightSignals = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this); + + sub_twist_raw = nh.subscribe("/twist_raw", 1, &BehaviorGen::callbackGetTwistRaw, this); + sub_twist_cmd = nh.subscribe("/twist_cmd", 1, &BehaviorGen::callbackGetTwistCMD, this); +@@ -206,9 +192,8 @@ void BehaviorGen::UpdatePlanningParams(ros::NodeHandle& _nh) + m_BehaviorGenerator.m_obstacleWaitingTimeinIntersection = m_obstacleWaitingTimeinIntersection; + } + +-void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ +- double distance = msg.data; +- if(distance < m_distanceToPedestrianThreshold){ ++void BehaviorGen::_callbackDistanceToPedestrian(){ ++ if(distance_to_pdestrian_ < m_distanceToPedestrianThreshold){ + m_PlanningParams.pedestrianAppearence = true; + } + else +@@ -216,7 +201,10 @@ void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ + m_PlanningParams.pedestrianAppearence = false; + } + m_BehaviorGenerator.UpdatePedestrianAppearence(m_PlanningParams.pedestrianAppearence); +- // m_BehaviorGenerator.printPedestrianAppearence(); ++} ++ ++void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ ++ distance_to_pdestrian_ = msg.data; + } + + void BehaviorGen::callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg){ +@@ -245,24 +233,6 @@ void BehaviorGen::callbackGetCommandCMD(const autoware_msgs::ControlCommandConst + m_Ctrl_cmd = *msg; + } + +-void BehaviorGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +-{ +- m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); +- bNewCurrentPos = true; +-} +- +-void BehaviorGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) +-{ +- m_VehicleStatus.speed = msg->twist.linear.x; +- m_CurrentPos.v = m_VehicleStatus.speed; +- if(fabs(msg->twist.linear.x) > 0.25) +- m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); +- UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); +- bVehicleStatus = true; +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +-} +- + void BehaviorGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) + { + m_VehicleStatus.speed = msg->speed/3.6; +@@ -373,17 +343,46 @@ void BehaviorGen::callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstP + m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity; + } + +-void BehaviorGen::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++void BehaviorGen::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) + { +- if(msg->lanes.size() > 0) ++ // Before spinOnce ++ rubis::start_task_profiling(); ++ rubis::instance_ = msg->instance; ++ rubis::obj_instance_ = msg->obj_instance; ++ ++ // Callback for distance to pedestrian ++ _callbackDistanceToPedestrian(); ++ ++ static double prev_x = 0.0, prev_y = 0.0, prev_speed = 0.0; ++ // Callback for current velocity ++ if(prev_speed != msg->twist.twist.linear.x){ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ prev_speed = msg->twist.twist.linear.x; ++ } ++ ++ // Callback for current pose ++ if(prev_x != msg->pose.pose.position.x || prev_y != msg->pose.pose.position.y){ ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ bNewCurrentPos = true; ++ prev_x = msg->pose.pose.position.x; ++ prev_y = msg->pose.pose.position.y; ++ } ++ ++ // Callback for local planner path ++ if(msg->lane_array.lanes.size() > 0) + { + m_RollOuts.clear(); + int globalPathId_roll_outs = -1; + +- for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) + { + std::vector path; +- PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), path); ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); + m_RollOuts.push_back(path); + + if(path.size() > 0) +@@ -410,6 +409,143 @@ void BehaviorGen::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayCons + m_BehaviorGenerator.m_RollOuts = m_RollOuts; + bRollOuts = true; + } ++ ++ // Main Loop ++ // Check Pedestrian is Appeared ++ double dt = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer); ++ UtilityHNS::UtilityH::GetTickCount(planningTimer); ++ ++ if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); ++ } ++ else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); ++ ++ } ++ else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) ++ { ++ std::vector conn_data;; ++ ++ if(m_MapRaw.GetVersion()==2) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, ++ m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); ++ ++ try{ ++ // Add Traffic Signal Info from yaml file ++ XmlRpc::XmlRpcValue traffic_light_list; ++ nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list); ++ ++ // Add Stop Line Info from yaml file ++ XmlRpc::XmlRpcValue stop_line_list; ++ nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list); ++ ++ // Add Crossing Info from yaml file ++ // XmlRpc::XmlRpcValue intersection_list; ++ // nh.getParam("/op_behavior_selector/intersection_list", intersection_list); ++ ++ PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list); ++ } ++ catch(XmlRpc::XmlRpcException& e){ ++ ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str()); ++ exit(1); ++ } ++ ++ m_BehaviorGenerator.m_Map = m_Map; ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl; ++ } ++ } ++ else if(m_MapRaw.GetVersion()==1) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl; ++ } ++ } ++ } ++ ++ if(bNewCurrentPos && m_GlobalPaths.size()>0) ++ { ++ if(bNewLightSignal) ++ { ++ m_PrevTrafficLight = m_CurrTrafficLight; ++ bNewLightSignal = false; ++ } ++ ++ if(bNewLightStatus) ++ { ++ bNewLightStatus = false; ++ for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++) ++ m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus; ++ } ++ ++ m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch; ++ m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0); ++ std_msgs::Int32 curr_state_msg; ++ curr_state_msg.data = m_CurrentBehavior.state; ++ ++ pub_currentState.publish(curr_state_msg); ++ ++ CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint); ++ m_BehaviorGenerator.m_turnAngle = m_turnAngle; ++ ++ std_msgs::Float64 turn_angle_msg; ++ turn_angle_msg.data = m_turnAngle; ++ pub_turnAngle.publish(turn_angle_msg); ++ ++ emergency_stop_msg.data = false; ++ if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop! ++ emergency_stop_msg.data = true; ++ pub_EmergencyStop.publish(emergency_stop_msg); ++ ++ SendLocalPlanningTopics(msg); ++ VisualizeLocalPlanner(); ++ LogLocalPlanningInfo(dt); ++ ++ // Publish turn_marker ++ visualization_msgs::MarkerArray turn_marker; ++ visualization_msgs::Marker marker; ++ marker.header.frame_id = "map"; ++ marker.type = 2; ++ marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x; ++ marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y; ++ marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z; ++ marker.scale.x = 3; ++ marker.scale.y = 3; ++ marker.scale.z = 3; ++ marker.color.r = 0.0f; ++ marker.color.g = 1.0f; ++ marker.color.b = 0.0f; ++ marker.color.a = 1.0f; ++ marker.header.stamp = ros::Time::now(); ++ marker.header.frame_id = "map"; ++ turn_marker.markers.push_back(marker); ++ ++ pub_turnMarker.publish(turn_marker); ++ } ++ else ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + void BehaviorGen::callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg) +@@ -481,7 +617,7 @@ void BehaviorGen::VisualizeLocalPlanner() + // pub_SelectedPathRviz.publish(selected_path); + } + +-void BehaviorGen::SendLocalPlanningTopics() ++void BehaviorGen::SendLocalPlanningTopics(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) + { + //Send Behavior State + geometry_msgs::Twist t; +@@ -523,13 +659,23 @@ void BehaviorGen::SendLocalPlanningTopics() + PlannerHNS::RelativeInfo info; + PlannerHNS::PlanningHelpers::GetRelativeInfo(m_BehaviorGenerator.m_Path, m_BehaviorGenerator.state, info); + PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_BehaviorGenerator.m_Path, m_CurrentTrajectoryToSend, info.iBack); +- //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl; ++ //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl; + + closest_waypoint.data = 1; + pub_ClosestIndex.publish(closest_waypoint); + pub_LocalBasePath.publish(m_CurrentTrajectoryToSend); ++ ++ rubis_msgs::LaneWithPoseTwist final_waypoints_with_pose_twist_msg; ++ final_waypoints_with_pose_twist_msg.instance = rubis::instance_; ++ final_waypoints_with_pose_twist_msg.obj_instance = rubis::obj_instance_; ++ final_waypoints_with_pose_twist_msg.lane = m_CurrentTrajectoryToSend; ++ final_waypoints_with_pose_twist_msg.pose = msg->pose; ++ final_waypoints_with_pose_twist_msg.twist = msg->twist; ++ ++ pub_LocalPathWithPosePub.publish(final_waypoints_with_pose_twist_msg); + pub_LocalPath.publish(m_CurrentTrajectoryToSend); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ ++ + } + + void BehaviorGen::LogLocalPlanningInfo(double dt) +@@ -606,195 +752,32 @@ void BehaviorGen::MainLoop() + { + ros::NodeHandle private_nh("~"); + +- timespec planningTimer; +- UtilityHNS::UtilityH::GetTickCount(planningTimer); +- std_msgs::Bool emergency_stop_msg; +- +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- + m_BehaviorGenerator.m_turnThreshold = m_turnThreshold; +- +-m_sprintSwitch = false; +- +- private_nh.param("/op_behavior_selector/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_behavior_selector/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/op_behavior_selector/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv"); +- private_nh.param("/op_behavior_selector/rate", rate, 10); +- private_nh.param("/op_behavior_selector/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/op_behavior_selector/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/op_behavior_selector/task_relative_deadline", task_relative_deadline, (double)10); +- +- /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25); +- +- struct timespec start_time, end_time; +- + m_sprintSwitch = false; + +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- // Check Pedestrian is Appeared +- double dt = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer); +- UtilityHNS::UtilityH::GetTickCount(planningTimer); +- +- if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) +- { +- bMap = true; +- PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); +- } +- else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) +- { +- bMap = true; +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); +- +- } +- else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) +- { +- std::vector conn_data;; +- +- if(m_MapRaw.GetVersion()==2) +- { +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, +- m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, +- m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, +- m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, +- m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, +- m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); +- +- try{ +- // Add Traffic Signal Info from yaml file +- XmlRpc::XmlRpcValue traffic_light_list; +- nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list); +- +- // Add Stop Line Info from yaml file +- XmlRpc::XmlRpcValue stop_line_list; +- nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list); +- +- // Add Crossing Info from yaml file +- // XmlRpc::XmlRpcValue intersection_list; +- // nh.getParam("/op_behavior_selector/intersection_list", intersection_list); +- +- PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list); +- } +- catch(XmlRpc::XmlRpcException& e){ +- ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str()); +- exit(1); +- } +- +- m_BehaviorGenerator.m_Map = m_Map; +- +- if(m_Map.roadSegments.size() > 0) +- { +- bMap = true; +- std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl; +- } +- } +- else if(m_MapRaw.GetVersion()==1) +- { +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, +- m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, +- m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, +- m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, +- m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); +- +- if(m_Map.roadSegments.size() > 0) +- { +- bMap = true; +- std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl; +- } +- } +- } +- +- if(bNewCurrentPos && m_GlobalPaths.size()>0) +- { +- if(bNewLightSignal) +- { +- m_PrevTrafficLight = m_CurrTrafficLight; +- bNewLightSignal = false; +- } +- +- if(bNewLightStatus) +- { +- bNewLightStatus = false; +- for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++) +- m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus; +- } +- +- m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch; +- m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0); +- std_msgs::Int32 curr_state_msg; +- curr_state_msg.data = m_CurrentBehavior.state; +- +- pub_currentState.publish(curr_state_msg); +- +- CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint); +- m_BehaviorGenerator.m_turnAngle = m_turnAngle; +- +- std_msgs::Float64 turn_angle_msg; +- turn_angle_msg.data = m_turnAngle; +- pub_turnAngle.publish(turn_angle_msg); +- +- emergency_stop_msg.data = false; +- if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop! +- emergency_stop_msg.data = true; +- pub_EmergencyStop.publish(emergency_stop_msg); +- +- SendLocalPlanningTopics(); +- VisualizeLocalPlanner(); +- LogLocalPlanningInfo(dt); +- +- // Publish turn_marker +- visualization_msgs::MarkerArray turn_marker; +- visualization_msgs::Marker marker; +- marker.header.frame_id = "map"; +- marker.type = 2; +- marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x; +- marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y; +- marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z; +- marker.scale.x = 3; +- marker.scale.y = 3; +- marker.scale.z = 3; +- marker.color.r = 0.0f; +- marker.color.g = 1.0f; +- marker.color.b = 0.0f; +- marker.color.a = 1.0f; +- marker.header.stamp = ros::Time::now(); +- marker.header.frame_id = "map"; +- turn_marker.markers.push_back(marker); +- +- pub_turnMarker.publish(turn_marker); +- } +- else +- sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); ++ std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv"); + +- loop_rate.sleep(); +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + } + + bool BehaviorGen::GetBaseMapTF(){ +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp +new file mode 100644 +index 00000000..250edb07 +--- /dev/null ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_behavior_selector/op_behavior_selector_core.mainloop.cpp +@@ -0,0 +1,905 @@ ++/* ++ * Copyright 2018-2019 Autoware Foundation. All rights reserved. ++ * ++ * Licensed under the Apache License, Version 2.0 (the "License"); ++ * you may not use this file except in compliance with the License. ++ * You may obtain a copy of the License at ++ * ++ * http://www.apache.org/licenses/LICENSE-2.0 ++ * ++ * Unless required by applicable law or agreed to in writing, software ++ * distributed under the License is distributed on an "AS IS" BASIS, ++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ++ * See the License for the specific language governing permissions and ++ * limitations under the License. ++ */ ++ ++#include "op_behavior_selector_core.h" ++#include "op_ros_helpers/op_ROSHelpers.h" ++#include "op_planner/MappingHelpers.h" ++#include ++ ++namespace BehaviorGeneratorNS ++{ ++ ++BehaviorGen::BehaviorGen() ++{ ++ sleep(2); ++ bNewCurrentPos = false; ++ bVehicleStatus = false; ++ bWayGlobalPath = false; ++ bWayGlobalPathLogs = false; ++ bNewLightStatus = false; ++ bNewLightSignal = false; ++ bBestCost = false; ++ bMap = false; ++ bRollOuts = false; ++ UtilityHNS::UtilityH::GetTickCount(planningTimer); ++ ++ ros::NodeHandle _nh; ++ UpdatePlanningParams(_nh); ++ ++ // RUBIS driving parameter ++ nh.getParam("/op_behavior_selector/distanceToPedestrianThreshold", m_distanceToPedestrianThreshold); ++ nh.param("/op_behavior_selector/turnThreshold", m_turnThreshold, 20.0); ++ ++ ++ tf::StampedTransform transform; ++ PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); ++ m_OriginPos.position.x = transform.getOrigin().x(); ++ m_OriginPos.position.y = transform.getOrigin().y(); ++ m_OriginPos.position.z = transform.getOrigin().z(); ++ ++ pub_LocalPath = nh.advertise("final_waypoints", 1,true); ++ pub_LocalPathWithPosePub = nh.advertise("final_waypoints_with_pose_twist", 1,true); ++ pub_LocalBasePath = nh.advertise("base_waypoints", 1,true); ++ pub_ClosestIndex = nh.advertise("closest_waypoint", 1,true); ++ pub_BehaviorState = nh.advertise("current_behavior", 1); ++ pub_SimuBoxPose = nh.advertise("sim_box_pose_ego", 1); ++ pub_BehaviorStateRviz = nh.advertise("behavior_state", 1); ++ pub_SelectedPathRviz = nh.advertise("local_selected_trajectory_rviz", 1); ++ pub_EmergencyStop = nh.advertise("emergency_stop", 1); ++ pub_turnAngle = nh.advertise("turn_angle", 1); ++ pub_turnMarker = nh.advertise("turn_marker", 1); ++ pub_currentState = nh.advertise("current_state", 1); ++ ++ int bVelSource = 1; ++ _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); ++ if(bVelSource == 0) ++ sub_robot_odom = nh.subscribe("/odom", 10, &BehaviorGen::callbackGetRobotOdom, this); ++ else if(bVelSource == 2) ++ sub_can_info = nh.subscribe("/can_info", 10, &BehaviorGen::callbackGetCANInfo, this); ++ ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); ++ sub_LocalPlannerPaths = nh.subscribe("/local_weighted_trajectories_with_pose_twist", 1, &BehaviorGen::callbackGetLocalPlannerPath, this); ++ // sub_TrafficLightStatus = nh.subscribe("/light_color", 1, &BehaviorGen::callbackGetTrafficLightStatus, this); ++ // sub_TrafficLightSignals = nh.subscribe("/roi_signal", 1, &BehaviorGen::callbackGetTrafficLightSignals, this); ++ sub_Trajectory_Cost = nh.subscribe("/local_trajectory_cost", 1, &BehaviorGen::callbackGetLocalTrajectoryCost, this); ++ ++ sub_TrafficLightSignals = nh.subscribe("/v2x_traffic_signal", 1, &BehaviorGen::callbackGetV2XTrafficLightSignals, this); ++ ++ sub_twist_raw = nh.subscribe("/twist_raw", 1, &BehaviorGen::callbackGetTwistRaw, this); ++ sub_twist_cmd = nh.subscribe("/twist_cmd", 1, &BehaviorGen::callbackGetTwistCMD, this); ++ //sub_ctrl_cmd = nh.subscribe("/ctrl_cmd", 1, &BehaviorGen::callbackGetCommandCMD, this); ++ sub_DistanceToPedestrian = nh.subscribe("/distance_to_pedestrian", 1, &BehaviorGen::callbackDistanceToPedestrian, this); ++ sub_IntersectionCondition = nh.subscribe("/intersection_condition", 1, &BehaviorGen::callbackIntersectionCondition, this); ++ sub_SprintSwitch = nh.subscribe("/sprint_switch", 1, &BehaviorGen::callbackSprintSwitch, this); ++ ++ //Mapping Section ++ sub_lanes = nh.subscribe("/vector_map_info/lane", 1, &BehaviorGen::callbackGetVMLanes, this); ++ sub_points = nh.subscribe("/vector_map_info/point", 1, &BehaviorGen::callbackGetVMPoints, this); ++ sub_dt_lanes = nh.subscribe("/vector_map_info/dtlane", 1, &BehaviorGen::callbackGetVMdtLanes, this); ++ sub_intersect = nh.subscribe("/vector_map_info/cross_road", 1, &BehaviorGen::callbackGetVMIntersections, this); ++ sup_area = nh.subscribe("/vector_map_info/area", 1, &BehaviorGen::callbackGetVMAreas, this); ++ sub_lines = nh.subscribe("/vector_map_info/line", 1, &BehaviorGen::callbackGetVMLines, this); ++ sub_stop_line = nh.subscribe("/vector_map_info/stop_line", 1, &BehaviorGen::callbackGetVMStopLines, this); ++ sub_signals = nh.subscribe("/vector_map_info/signal", 1, &BehaviorGen::callbackGetVMSignal, this); ++ sub_vectors = nh.subscribe("/vector_map_info/vector", 1, &BehaviorGen::callbackGetVMVectors, this); ++ sub_curbs = nh.subscribe("/vector_map_info/curb", 1, &BehaviorGen::callbackGetVMCurbs, this); ++ sub_edges = nh.subscribe("/vector_map_info/road_edge", 1, &BehaviorGen::callbackGetVMRoadEdges, this); ++ sub_way_areas = nh.subscribe("/vector_map_info/way_area", 1, &BehaviorGen::callbackGetVMWayAreas, this); ++ sub_cross_walk = nh.subscribe("/vector_map_info/cross_walk", 1, &BehaviorGen::callbackGetVMCrossWalks, this); ++ sub_nodes = nh.subscribe("/vector_map_info/node", 1, &BehaviorGen::callbackGetVMNodes, this); ++} ++ ++BehaviorGen::~BehaviorGen() ++{ ++ UtilityHNS::DataRW::WriteLogData(UtilityHNS::UtilityH::GetHomeDirectory()+UtilityHNS::DataRW::LoggingMainfolderName+UtilityHNS::DataRW::StatesLogFolderName, "MainLog", ++ "time,dt, Behavior_i, Behavior_str, RollOuts_n, Blocked_i, Central_i, Selected_i, StopSign_id, Light_id, Stop_Dist, Follow_Dist, Follow_Vel," ++ "Target_Vel, PID_Vel, T_cmd_Vel, C_cmd_Vel, Vel, Steer, X, Y, Z, Theta," ++ , m_LogData); ++} ++ ++void BehaviorGen::UpdatePlanningParams(ros::NodeHandle& _nh) ++{ ++ _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); ++ if(m_PlanningParams.enableSwerving) ++ m_PlanningParams.enableFollowing = true; ++ else ++ _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); ++ ++ _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); ++ _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); ++ ++ _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); ++ _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); ++ _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); ++ ++ _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); ++ ++ _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); ++ if(m_PlanningParams.enableSwerving) ++ _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); ++ else ++ m_PlanningParams.rollOutNumber = 0; ++ ++ _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); ++ _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); ++ _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); ++ _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); ++ _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); ++ ++ _nh.getParam("/op_trajectory_evaluator/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); ++ _nh.getParam("/op_trajectory_evaluator/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); ++ ++ _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); ++ ++ _nh.getParam("/op_common_params/width", m_CarInfo.width); ++ _nh.getParam("/op_common_params/length", m_CarInfo.length); ++ _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); ++ _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); ++ _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); ++ _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); ++ _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); ++ m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; ++ m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; ++ ++ _nh.getParam("/op_common_params/stopLineMargin", m_PlanningParams.stopLineMargin); ++ _nh.getParam("/op_common_params/stopLineDetectionDistance", m_PlanningParams.stopLineDetectionDistance); ++ ++ PlannerHNS::ControllerParams controlParams; ++ controlParams.Steering_Gain = PlannerHNS::PID_CONST(0.07, 0.02, 0.01); ++ controlParams.Velocity_Gain = PlannerHNS::PID_CONST(0.1, 0.005, 0.1); ++ nh.getParam("/op_common_params/steeringDelay", controlParams.SteeringDelay); ++ nh.getParam("/op_common_params/minPursuiteDistance", controlParams.minPursuiteDistance ); ++ nh.getParam("/op_common_params/additionalBrakingDistance", m_PlanningParams.additionalBrakingDistance ); ++ nh.getParam("/op_common_params/giveUpDistance", m_PlanningParams.giveUpDistance ); ++ nh.getParam("/op_common_params/enableSlowDownOnCurve", m_PlanningParams.enableSlowDownOnCurve ); ++ nh.getParam("/op_common_params/curveVelocityRatio", m_PlanningParams.curveVelocityRatio ); ++ ++ int iSource = 0; ++ _nh.getParam("/op_common_params/mapSource" , iSource); ++ if(iSource == 0) ++ m_MapType = PlannerHNS::MAP_AUTOWARE; ++ else if (iSource == 1) ++ m_MapType = PlannerHNS::MAP_FOLDER; ++ else if(iSource == 2) ++ m_MapType = PlannerHNS::MAP_KML_FILE; ++ ++ _nh.getParam("/op_common_params/mapFileName" , m_MapPath); ++ ++ _nh.getParam("/op_behavior_selector/evidence_tust_number", m_PlanningParams.nReliableCount); ++ ++ //std::cout << "nReliableCount: " << m_PlanningParams.nReliableCount << std::endl; ++ ++ _nh.param("/op_behavior_selector/sprintSpeed", m_sprintSpeed, 13.5); ++ _nh.param("/op_behavior_selector/obstacleWaitingTimeinIntersection", m_obstacleWaitingTimeinIntersection, 1.0); ++ ++ m_BehaviorGenerator.Init(controlParams, m_PlanningParams, m_CarInfo, m_sprintSpeed); ++ ++ m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior = PlannerHNS::INITIAL_STATE; ++ ++ m_BehaviorGenerator.m_obstacleWaitingTimeinIntersection = m_obstacleWaitingTimeinIntersection; ++} ++ ++void BehaviorGen::callbackDistanceToPedestrian(const std_msgs::Float64& msg){ ++ double distance = msg.data; ++ if(distance < m_distanceToPedestrianThreshold){ ++ m_PlanningParams.pedestrianAppearence = true; ++ } ++ else ++ { ++ m_PlanningParams.pedestrianAppearence = false; ++ } ++ m_BehaviorGenerator.UpdatePedestrianAppearence(m_PlanningParams.pedestrianAppearence); ++ // m_BehaviorGenerator.printPedestrianAppearence(); ++} ++ ++void BehaviorGen::callbackIntersectionCondition(const autoware_msgs::IntersectionCondition& msg){ ++ m_BehaviorGenerator.m_isInsideIntersection = msg.isIntersection; ++ m_BehaviorGenerator.m_closestIntersectionDistance = msg.intersectionDistance; ++ m_BehaviorGenerator.m_riskyLeft = msg.riskyLeftTurn; ++ m_BehaviorGenerator.m_riskyRight = msg.riskyRightTurn; ++} ++ ++void BehaviorGen::callbackSprintSwitch(const std_msgs::Bool& msg){ ++ m_sprintSwitch = msg.data; ++} ++ ++void BehaviorGen::callbackGetTwistRaw(const geometry_msgs::TwistStampedConstPtr& msg) ++{ ++ m_Twist_raw = *msg; ++} ++ ++void BehaviorGen::callbackGetTwistCMD(const geometry_msgs::TwistStampedConstPtr& msg) ++{ ++ m_Twist_cmd = *msg; ++} ++ ++void BehaviorGen::callbackGetCommandCMD(const autoware_msgs::ControlCommandConstPtr& msg) ++{ ++ m_Ctrl_cmd = *msg; ++} ++ ++void BehaviorGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) ++{ ++ m_VehicleStatus.speed = msg->speed/3.6; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void BehaviorGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) ++{ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed ; ++ if(msg->twist.twist.linear.x != 0) ++ m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void BehaviorGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++{ ++ if(msg->lanes.size() > 0 && bMap) ++ { ++ ++ bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); ++ ++ m_GlobalPaths.clear(); ++ ++ for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ { ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); ++ PlannerHNS::Lane* pPrevValid = 0; ++ for(unsigned int j = 0 ; j < m_temp_path.size(); j++) ++ { ++ PlannerHNS::Lane* pLane = 0; ++ pLane = PlannerHNS::MappingHelpers::GetLaneById(m_temp_path.at(j).laneId, m_Map); ++ if(!pLane) ++ { ++ pLane = PlannerHNS::MappingHelpers::GetClosestLaneFromMapDirectionBased(m_temp_path.at(j), m_Map, 1); ++ ++ if(!pLane && !pPrevValid) ++ { ++ ROS_ERROR("Map inconsistency between Global Path and Local Planer Map, Can't identify current lane."); ++ return; ++ } ++ ++ if(!pLane) ++ m_temp_path.at(j).pLane = pPrevValid; ++ else ++ { ++ m_temp_path.at(j).pLane = pLane; ++ pPrevValid = pLane ; ++ } ++ ++ m_temp_path.at(j).laneId = m_temp_path.at(j).pLane->id; ++ } ++ else ++ m_temp_path.at(j).pLane = pLane; ++ ++ ++ //std::cout << "StopLineInGlobalPath: " << m_temp_path.at(j).stopLineID << std::endl; ++ } ++ ++ PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); ++ m_GlobalPaths.push_back(m_temp_path); ++ ++ if(bOldGlobalPath) ++ { ++ bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); ++ } ++ } ++ ++ if(!bOldGlobalPath) ++ { ++ bWayGlobalPath = true; ++ bWayGlobalPathLogs = true; ++ for(unsigned int i = 0; i < m_GlobalPaths.size(); i++) ++ { ++ PlannerHNS::PlanningHelpers::FixPathDensity(m_GlobalPaths.at(i), m_PlanningParams.pathDensity); ++ PlannerHNS::PlanningHelpers::SmoothPath(m_GlobalPaths.at(i), 0.35, 0.4, 0.05); ++ ++ PlannerHNS::PlanningHelpers::GenerateRecommendedSpeed(m_GlobalPaths.at(i), m_CarInfo.max_speed_forward, m_PlanningParams.speedProfileFactor); ++ m_GlobalPaths.at(i).at(m_GlobalPaths.at(i).size()-1).v = 0; ++ } ++ ++ std::cout << "Received New Global Path Selector! " << std::endl; ++ } ++ else ++ { ++ m_GlobalPaths.clear(); ++ } ++ } ++} ++ ++void BehaviorGen::callbackGetLocalTrajectoryCost(const autoware_msgs::LaneConstPtr& msg) ++{ ++ if(m_BehaviorGenerator.m_pCurrentBehaviorState->m_Behavior == PlannerHNS::INTERSECTION_STATE){ ++ bBestCost = true; ++ m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance; ++ m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity; ++ return; ++ } ++ bBestCost = true; ++ m_TrajectoryBestCost.bBlocked = msg->is_blocked; ++ m_TrajectoryBestCost.index = msg->lane_index; ++ m_TrajectoryBestCost.cost = msg->cost; ++ m_TrajectoryBestCost.closest_obj_distance = msg->closest_object_distance; ++ m_TrajectoryBestCost.closest_obj_velocity = msg->closest_object_velocity; ++} ++ ++void BehaviorGen::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) ++{ ++ rubis::instance_ = msg->instance; ++ lane_array_with_pose_twist_msg_ = msg; ++ ++ // Callback ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ bNewCurrentPos = true; ++ ++ if(msg->lane_array.lanes.size() > 0) ++ { ++ m_RollOuts.clear(); ++ int globalPathId_roll_outs = -1; ++ ++ for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) ++ { ++ std::vector path; ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); ++ m_RollOuts.push_back(path); ++ ++ if(path.size() > 0) ++ globalPathId_roll_outs = path.at(0).gid; ++ } ++ ++ if(bWayGlobalPath && m_GlobalPaths.size() > 0) ++ { ++ if(m_GlobalPaths.at(0).size() > 0) ++ { ++ int globalPathId = m_GlobalPaths.at(0).at(0).gid; ++ std::cout << "Before Synchronization At Behavior Selector: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; ++ ++ if(globalPathId_roll_outs == globalPathId) ++ { ++ bWayGlobalPath = false; ++ m_GlobalPathsToUse = m_GlobalPaths; ++ m_BehaviorGenerator.SetNewGlobalPath(m_GlobalPathsToUse); ++ std::cout << "Synchronization At Behavior Selector: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; ++ } ++ } ++ } ++ ++ m_BehaviorGenerator.m_RollOuts = m_RollOuts; ++ bRollOuts = true; ++ } ++} ++ ++void BehaviorGen::callbackGetV2XTrafficLightSignals(const autoware_msgs::RUBISTrafficSignalArray& msg) ++{ ++ bNewLightSignal = true; ++ std::vector simulatedLights; ++ for(unsigned int i = 0 ; i < msg.signals.size() ; i++) ++ { ++ PlannerHNS::TrafficLight tl; ++ tl.id = msg.signals.at(i).id; ++ tl.remainTime = msg.signals.at(i).time; ++ ++ for(unsigned int k = 0; k < m_Map.trafficLights.size(); k++) ++ { ++ if(m_Map.trafficLights.at(k).id == tl.id) ++ { ++ tl.pos = m_Map.trafficLights.at(k).pos; ++ tl.routine = m_Map.trafficLights.at(k).routine; ++ break; ++ } ++ } ++ ++ if(msg.signals.at(i).type == 0) ++ { ++ tl.lightState = PlannerHNS::RED_LIGHT; ++ } ++ else if(msg.signals.at(i).type == 1) ++ { ++ tl.lightState = PlannerHNS::YELLOW_LIGHT; ++ } ++ else ++ { ++ tl.lightState = PlannerHNS::GREEN_LIGHT; ++ } ++ ++ simulatedLights.push_back(tl); ++ } ++ ++ m_CurrTrafficLight = simulatedLights; ++} ++ ++void BehaviorGen::VisualizeLocalPlanner() ++{ ++ visualization_msgs::Marker behavior_rviz; ++ int iDirection = 0; ++ if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory > m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory) ++ iDirection = 1; ++ else if(m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory < m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory) ++ iDirection = -1; ++ PlannerHNS::ROSHelpers::VisualizeBehaviorState(m_CurrentPos, m_CurrentBehavior, !m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bTrafficIsRed , iDirection, behavior_rviz, "beh_state"); ++ //pub_BehaviorStateRviz.publish(behavior_rviz); ++ ++ visualization_msgs::MarkerArray markerArray; ++ ++ //PlannerHNS::ROSHelpers::GetIndicatorArrows(m_CurrentPos, m_CarInfo.width, m_CarInfo.length, m_CurrentBehavior.indicator, 0, markerArray); ++ ++ markerArray.markers.push_back(behavior_rviz); ++ ++ pub_BehaviorStateRviz.publish(markerArray); ++ ++ //To Test Synchronization Problem ++// visualization_msgs::MarkerArray selected_path; ++// std::vector > > paths; ++// paths.push_back(std::vector >()); ++// paths.at(0).push_back(m_BehaviorGenerator.m_Path); ++// paths.push_back(m_GlobalPathsToUse); ++// paths.push_back(m_RollOuts); ++// PlannerHNS::ROSHelpers::TrajectoriesToMarkers(paths, selected_path); ++// pub_SelectedPathRviz.publish(selected_path); ++} ++ ++void BehaviorGen::SendLocalPlanningTopics(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) ++{ ++ //Send Behavior State ++ geometry_msgs::Twist t; ++ geometry_msgs::TwistStamped behavior; ++ t.linear.x = m_CurrentBehavior.bNewPlan; ++ t.linear.y = m_CurrentBehavior.followDistance; ++ t.linear.z = m_CurrentBehavior.followVelocity; ++ t.angular.x = (int)m_CurrentBehavior.indicator; ++ t.angular.y = (int)m_CurrentBehavior.state; ++ t.angular.z = m_CurrentBehavior.iTrajectory; ++ behavior.twist = t; ++ behavior.header.stamp = ros::Time::now(); ++ pub_BehaviorState.publish(behavior); ++ ++ //Send Ego Vehicle Simulation Pose Data ++ geometry_msgs::PoseArray sim_data; ++ geometry_msgs::Pose p_id, p_pose, p_box; ++ ++ sim_data.header.frame_id = "map"; ++ sim_data.header.stamp = ros::Time(); ++ p_id.position.x = 0; ++ p_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a)); ++ ++ PlannerHNS::WayPoint pose_center = PlannerHNS::PlanningHelpers::GetRealCenter(m_BehaviorGenerator.state, m_CarInfo.wheel_base); ++ ++ p_pose.position.x = pose_center.pos.x; ++ p_pose.position.y = pose_center.pos.y; ++ p_pose.position.z = pose_center.pos.z; ++ p_box.position.x = m_BehaviorGenerator.m_CarInfo.width; ++ p_box.position.y = m_BehaviorGenerator.m_CarInfo.length; ++ p_box.position.z = 2.2; ++ sim_data.poses.push_back(p_id); ++ sim_data.poses.push_back(p_pose); ++ sim_data.poses.push_back(p_box); ++ pub_SimuBoxPose.publish(sim_data); ++ ++ //Send Trajectory Data to path following nodes ++ std_msgs::Int32 closest_waypoint; ++ PlannerHNS::RelativeInfo info; ++ PlannerHNS::PlanningHelpers::GetRelativeInfo(m_BehaviorGenerator.m_Path, m_BehaviorGenerator.state, info); ++ PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_BehaviorGenerator.m_Path, m_CurrentTrajectoryToSend, info.iBack); ++ //std::cout << "Path Size: " << m_BehaviorGenerator.m_Path.size() << ", Send Size: " << m_CurrentTrajectoryToSend << std::endl; ++ ++ closest_waypoint.data = 1; ++ pub_ClosestIndex.publish(closest_waypoint); ++ pub_LocalBasePath.publish(m_CurrentTrajectoryToSend); ++ ++ rubis_msgs::LaneWithPoseTwist final_waypoints_with_pose_twist_msg; ++ final_waypoints_with_pose_twist_msg.instance = rubis::instance_; ++ final_waypoints_with_pose_twist_msg.lane = m_CurrentTrajectoryToSend; ++ final_waypoints_with_pose_twist_msg.pose = msg->pose; ++ final_waypoints_with_pose_twist_msg.twist = msg->twist; ++ ++ pub_LocalPathWithPosePub.publish(final_waypoints_with_pose_twist_msg); ++ pub_LocalPath.publish(m_CurrentTrajectoryToSend); ++ ++ ++} ++ ++void BehaviorGen::LogLocalPlanningInfo(double dt) ++{ ++ timespec log_t; ++ UtilityHNS::UtilityH::GetTickCount(log_t); ++ std::ostringstream dataLine; ++ dataLine << UtilityHNS::UtilityH::GetLongTime(log_t) <<"," << dt << "," << m_CurrentBehavior.state << ","<< PlannerHNS::ROSHelpers::GetBehaviorNameFromCode(m_CurrentBehavior.state) << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->m_pParams->rollOutNumber << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->bFullyBlock << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentStopSignID << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->currentTrafficLightID << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->minStoppingDistance << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->distanceToNext << "," << ++ m_BehaviorGenerator.m_pCurrentBehaviorState->GetCalcParams()->velocityOfNext << "," << ++ m_CurrentBehavior.maxVelocity << "," << ++ m_Twist_raw.twist.linear.x << "," << ++ m_Twist_cmd.twist.linear.x << "," << ++ m_Ctrl_cmd.linear_velocity << "," << ++ m_VehicleStatus.speed << "," << ++ m_VehicleStatus.steer << "," << ++ m_BehaviorGenerator.state.pos.x << "," << m_BehaviorGenerator.state.pos.y << "," << m_BehaviorGenerator.state.pos.z << "," << UtilityHNS::UtilityH::SplitPositiveAngle(m_BehaviorGenerator.state.pos.a)+M_PI << ","; ++ m_LogData.push_back(dataLine.str()); ++ ++ ++ if(bWayGlobalPathLogs) ++ { ++ for(unsigned int i=0; i < m_GlobalPaths.size(); i++) ++ { ++ std::ostringstream str_out; ++ str_out << UtilityHNS::UtilityH::GetHomeDirectory(); ++ str_out << UtilityHNS::DataRW::LoggingMainfolderName; ++ str_out << UtilityHNS::DataRW::PathLogFolderName; ++ str_out << "Global_Vel"; ++ str_out << i; ++ str_out << "_"; ++ PlannerHNS::PlanningHelpers::WritePathToFile(str_out.str(), m_GlobalPaths.at(i)); ++ } ++ bWayGlobalPathLogs = false; ++ } ++} ++ ++void BehaviorGen::CalculateTurnAngle(PlannerHNS::WayPoint turn_point){ ++ geometry_msgs::PoseStamped turn_pose; ++ ++ if(GetBaseMapTF()){ ++ // std::cout<<"BEFORE:"<("/op_behavior_selector/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_behavior_selector.csv"); ++ private_nh.param("/op_behavior_selector/rate", rate, 10); ++ private_nh.param("/op_behavior_selector/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); ++ private_nh.param("/op_behavior_selector/task_execution_time", task_execution_time, (double)10); ++ private_nh.param("/op_behavior_selector/task_relative_deadline", task_relative_deadline, (double)10); ++ ++ /* For Task scheduling */ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ m_sprintSwitch = false; ++ ++ ros::Rate r(100); ++ while(ros::ok()){ ++ rubis::start_task_profiling(); ++ ++ ros::spinOnce(); ++ ++ // Check Pedestrian is Appeared ++ double dt = UtilityHNS::UtilityH::GetTimeDiffNow(planningTimer); ++ UtilityHNS::UtilityH::GetTickCount(planningTimer); ++ ++ if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); ++ } ++ else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); ++ ++ } ++ else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) ++ { ++ std::vector conn_data;; ++ ++ if(m_MapRaw.GetVersion()==2) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, ++ m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); ++ ++ try{ ++ // Add Traffic Signal Info from yaml file ++ XmlRpc::XmlRpcValue traffic_light_list; ++ nh.getParam("/op_behavior_selector/traffic_light_list", traffic_light_list); ++ ++ // Add Stop Line Info from yaml file ++ XmlRpc::XmlRpcValue stop_line_list; ++ nh.getParam("/op_behavior_selector/stop_line_list", stop_line_list); ++ ++ // Add Crossing Info from yaml file ++ // XmlRpc::XmlRpcValue intersection_list; ++ // nh.getParam("/op_behavior_selector/intersection_list", intersection_list); ++ ++ PlannerHNS::MappingHelpers::ConstructRoadNetwork_RUBIS(m_Map, traffic_light_list, stop_line_list); ++ } ++ catch(XmlRpc::XmlRpcException& e){ ++ ROS_ERROR("[XmlRpc Error] %s", e.getMessage().c_str()); ++ exit(1); ++ } ++ ++ m_BehaviorGenerator.m_Map = m_Map; ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V2 Is Loaded successfully from the Behavior Selector !! " << std::endl; ++ } ++ } ++ else if(m_MapRaw.GetVersion()==1) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, false); ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V1 Is Loaded successfully from the Behavior Selector !! " << std::endl; ++ } ++ } ++ } ++ ++ if(bNewCurrentPos && m_GlobalPaths.size()>0) ++ { ++ if(bNewLightSignal) ++ { ++ m_PrevTrafficLight = m_CurrTrafficLight; ++ bNewLightSignal = false; ++ } ++ ++ if(bNewLightStatus) ++ { ++ bNewLightStatus = false; ++ for(unsigned int itls = 0 ; itls < m_PrevTrafficLight.size() ; itls++) ++ m_PrevTrafficLight.at(itls).lightState = m_CurrLightStatus; ++ } ++ ++ m_BehaviorGenerator.m_sprintSwitch = m_sprintSwitch; ++ m_CurrentBehavior = m_BehaviorGenerator.DoOneStep(dt, m_CurrentPos, m_VehicleStatus, 1, m_CurrTrafficLight, m_TrajectoryBestCost, 0); ++ std_msgs::Int32 curr_state_msg; ++ curr_state_msg.data = m_CurrentBehavior.state; ++ ++ pub_currentState.publish(curr_state_msg); ++ ++ CalculateTurnAngle(m_BehaviorGenerator.m_turnWaypoint); ++ m_BehaviorGenerator.m_turnAngle = m_turnAngle; ++ ++ std_msgs::Float64 turn_angle_msg; ++ turn_angle_msg.data = m_turnAngle; ++ pub_turnAngle.publish(turn_angle_msg); ++ ++ emergency_stop_msg.data = false; ++ if(m_CurrentBehavior.maxVelocity == -1)//Emergency Stop! ++ emergency_stop_msg.data = true; ++ pub_EmergencyStop.publish(emergency_stop_msg); ++ ++ SendLocalPlanningTopics(lane_array_with_pose_twist_msg_); ++ VisualizeLocalPlanner(); ++ LogLocalPlanningInfo(dt); ++ ++ // Publish turn_marker ++ visualization_msgs::MarkerArray turn_marker; ++ visualization_msgs::Marker marker; ++ marker.header.frame_id = "map"; ++ marker.type = 2; ++ marker.pose.position.x = m_BehaviorGenerator.m_turnWaypoint.pos.x; ++ marker.pose.position.y = m_BehaviorGenerator.m_turnWaypoint.pos.y; ++ marker.pose.position.z = m_BehaviorGenerator.m_turnWaypoint.pos.z; ++ marker.scale.x = 3; ++ marker.scale.y = 3; ++ marker.scale.z = 3; ++ marker.color.r = 0.0f; ++ marker.color.g = 1.0f; ++ marker.color.b = 0.0f; ++ marker.color.a = 1.0f; ++ marker.header.stamp = ros::Time::now(); ++ marker.header.frame_id = "map"; ++ turn_marker.markers.push_back(marker); ++ ++ pub_turnMarker.publish(turn_marker); ++ } ++ else ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &BehaviorGen::callbackGetGlobalPlannerPath, this); ++ ++ rubis::stop_task_profiling(0, 0); ++ r.sleep(); ++ } ++} ++ ++bool BehaviorGen::GetBaseMapTF(){ ++ ++ try{ ++ m_map_base_listener.waitForTransform("base_link", "map", ros::Time(0), ros::Duration(0.001)); ++ m_map_base_listener.lookupTransform("base_link", "map", ros::Time(0), m_map_base_transform); ++ return true; ++ } ++ catch(tf::TransformException& ex) ++ { ++ return false; ++ } ++ ++} ++ ++void BehaviorGen::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped& out_pose, const tf::StampedTransform &in_transform) ++{ ++ ++ tf::Vector3 in_pos(in_pose.pose.position.x, ++ in_pose.pose.position.y, ++ in_pose.pose.position.z); ++ tf::Quaternion in_quat(in_pose.pose.orientation.x, ++ in_pose.pose.orientation.y, ++ in_pose.pose.orientation.w, ++ in_pose.pose.orientation.z); ++ ++ tf::Vector3 in_pos_t = in_transform * in_pos; ++ tf::Quaternion in_quat_t = in_transform * in_quat; ++ ++ out_pose.header = in_pose.header; ++ out_pose.pose.position.x = in_pos_t.x(); ++ out_pose.pose.position.y = in_pos_t.y(); ++ out_pose.pose.position.z = in_pos_t.z(); ++ out_pose.pose.orientation.x = in_quat_t.x(); ++ out_pose.pose.orientation.y = in_quat_t.y(); ++ out_pose.pose.orientation.z = in_quat_t.z(); ++ ++ return; ++} ++ ++//Mapping Section ++ ++void BehaviorGen::callbackGetVMLanes(const vector_map_msgs::LaneArray& msg) ++{ ++ std::cout << "Received Lanes" << endl; ++ if(m_MapRaw.pLanes == nullptr) ++ m_MapRaw.pLanes = new UtilityHNS::AisanLanesFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMPoints(const vector_map_msgs::PointArray& msg) ++{ ++ std::cout << "Received Points" << endl; ++ if(m_MapRaw.pPoints == nullptr) ++ m_MapRaw.pPoints = new UtilityHNS::AisanPointsFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMdtLanes(const vector_map_msgs::DTLaneArray& msg) ++{ ++ std::cout << "Received dtLanes" << endl; ++ if(m_MapRaw.pCenterLines == nullptr) ++ m_MapRaw.pCenterLines = new UtilityHNS::AisanCenterLinesFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMIntersections(const vector_map_msgs::CrossRoadArray& msg) ++{ ++ std::cout << "Received CrossRoads" << endl; ++ if(m_MapRaw.pIntersections == nullptr) ++ m_MapRaw.pIntersections = new UtilityHNS::AisanIntersectionFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMAreas(const vector_map_msgs::AreaArray& msg) ++{ ++ std::cout << "Received Areas" << endl; ++ if(m_MapRaw.pAreas == nullptr) ++ m_MapRaw.pAreas = new UtilityHNS::AisanAreasFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMLines(const vector_map_msgs::LineArray& msg) ++{ ++ std::cout << "Received Lines" << endl; ++ if(m_MapRaw.pLines == nullptr) ++ m_MapRaw.pLines = new UtilityHNS::AisanLinesFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMStopLines(const vector_map_msgs::StopLineArray& msg) ++{ ++ std::cout << "Received StopLines" << endl; ++ if(m_MapRaw.pStopLines == nullptr) ++ m_MapRaw.pStopLines = new UtilityHNS::AisanStopLineFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMSignal(const vector_map_msgs::SignalArray& msg) ++{ ++ std::cout << "Received Signals" << endl; ++ if(m_MapRaw.pSignals == nullptr) ++ m_MapRaw.pSignals = new UtilityHNS::AisanSignalFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMVectors(const vector_map_msgs::VectorArray& msg) ++{ ++ std::cout << "Received Vectors" << endl; ++ if(m_MapRaw.pVectors == nullptr) ++ m_MapRaw.pVectors = new UtilityHNS::AisanVectorFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMCurbs(const vector_map_msgs::CurbArray& msg) ++{ ++ std::cout << "Received Curbs" << endl; ++ if(m_MapRaw.pCurbs == nullptr) ++ m_MapRaw.pCurbs = new UtilityHNS::AisanCurbFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMRoadEdges(const vector_map_msgs::RoadEdgeArray& msg) ++{ ++ std::cout << "Received Edges" << endl; ++ if(m_MapRaw.pRoadedges == nullptr) ++ m_MapRaw.pRoadedges = new UtilityHNS::AisanRoadEdgeFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMWayAreas(const vector_map_msgs::WayAreaArray& msg) ++{ ++ std::cout << "Received Wayareas" << endl; ++ if(m_MapRaw.pWayAreas == nullptr) ++ m_MapRaw.pWayAreas = new UtilityHNS::AisanWayareaFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMCrossWalks(const vector_map_msgs::CrossWalkArray& msg) ++{ ++ std::cout << "Received CrossWalks" << endl; ++ if(m_MapRaw.pCrossWalks == nullptr) ++ m_MapRaw.pCrossWalks = new UtilityHNS::AisanCrossWalkFileReader(msg); ++} ++ ++void BehaviorGen::callbackGetVMNodes(const vector_map_msgs::NodeArray& msg) ++{ ++ std::cout << "Received Nodes" << endl; ++ if(m_MapRaw.pNodes == nullptr) ++ m_MapRaw.pNodes = new UtilityHNS::AisanNodesFileReader(msg); ++} ++} +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp +index d31d230c..b4b55585 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_motion_predictor/op_motion_predictor_core.cpp +@@ -76,16 +76,16 @@ MotionPrediction::MotionPrediction() + object_msg_list_.push_back(msg); + } + +- sub_current_pose = nh.subscribe("/current_pose", 10, &MotionPrediction::callbackGetCurrentPose, this); ++ sub_current_pose = nh.subscribe("/current_pose", 1, &MotionPrediction::callbackGetCurrentPose, this); // Def: 1 + + int bVelSource = 1; + _nh.getParam("/op_motion_predictor/velocitySource", bVelSource); + if(bVelSource == 0) +- sub_robot_odom = nh.subscribe("/odom", 10, &MotionPrediction::callbackGetRobotOdom, this); ++ sub_robot_odom = nh.subscribe("/odom", 1, &MotionPrediction::callbackGetRobotOdom, this); // Def: 1 + else if(bVelSource == 1) +- sub_current_velocity = nh.subscribe("/current_velocity", 10, &MotionPrediction::callbackGetVehicleStatus, this); ++ sub_current_velocity = nh.subscribe("/current_velocity", 1, &MotionPrediction::callbackGetVehicleStatus, this); // Def: 1 + else if(bVelSource == 2) +- sub_can_info = nh.subscribe("/can_info", 10, &MotionPrediction::callbackGetCANInfo, this); ++ sub_can_info = nh.subscribe("/can_info", 1, &MotionPrediction::callbackGetCANInfo, this); // Def: 1 + + UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer); + PlannerHNS::ROSHelpers::InitPredMarkers(100, m_PredictedTrajectoriesDummy); +@@ -245,8 +245,6 @@ void MotionPrediction::callbackGetVehicleStatus(const geometry_msgs::TwistStampe + m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); + } + + void MotionPrediction::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) +@@ -405,16 +403,77 @@ void MotionPrediction::callbackGetTrackedObjects(const autoware_msgs::DetectedOb + } + } + +-void MotionPrediction::callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& in_msg) ++void MotionPrediction::callbackGetRubisTrackedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& in_msg){ ++ rubis::start_task_profiling(); ++ ++ rubis_msgs::DetectedObjectArray msg = *in_msg; ++ _callbackGetRubisTrackedObjects(msg); ++ ++ if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); ++ } ++ else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) ++ { ++ bMap = true; ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); ++ } ++ else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) ++ { ++ std::vector conn_data;; ++ ++ if(m_MapRaw.GetVersion()==2) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, ++ m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, true); ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V2 Is Loaded successfully from the Motion Predictor !! " << std::endl; ++ } ++ } ++ else if(m_MapRaw.GetVersion()==1) ++ { ++ PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, ++ m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, ++ m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, ++ m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, ++ m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true); ++ ++ if(m_Map.roadSegments.size() > 0) ++ { ++ bMap = true; ++ std::cout << " ******* Map V1 Is Loaded successfully from the Motion Predictor !! " << std::endl; ++ } ++ } ++ } ++ ++ if(UtilityHNS::UtilityH::GetTimeDiffNow(m_VisualizationTimer) > m_VisualizationTime) ++ { ++ VisualizePrediction(); ++ UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer); ++ } ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++} ++ ++void MotionPrediction::_callbackGetRubisTrackedObjects(rubis_msgs::DetectedObjectArray& objects_msg) + { +- rubis::instance_ = in_msg->instance; ++ rubis::instance_ = objects_msg.instance; ++ rubis::obj_instance_ = objects_msg.instance; + + UtilityHNS::UtilityH::GetTickCount(m_SensingTimer); + m_TrackedObjects.clear(); + bTrackedObjects = true; + + // Check frame id of the object is valid +- std::string target_frame = in_msg->object_array.header.frame_id; ++ std::string target_frame = objects_msg.object_array.header.frame_id; + int obj_idx = getIndex(tf_str_list_, target_frame); + if(obj_idx == -1){ + std::cout<object_array, transform_list_[obj_idx]); ++ msg = TrasformObjAryToVeldoyne(objects_msg.object_array, transform_list_[obj_idx]); + } + else{ +- msg = in_msg->object_array; ++ msg = objects_msg.object_array; + } + + +@@ -496,9 +555,10 @@ void MotionPrediction::callbackGetRubisTrackedObjects(const rubis_msgs::Detected + + rubis_msgs::DetectedObjectArray output_msg; + output_msg.instance = rubis::instance_; ++ output_msg.obj_instance = rubis::obj_instance_; + output_msg.object_array = m_PredictedResultsResults; + pub_rubis_predicted_objects_trajectories.publish(output_msg); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ + } + + } +@@ -643,99 +703,29 @@ void MotionPrediction::MainLoop() + + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- +- private_nh.param("/op_motion_predictor/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_motion_predictor/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/op_motion_predictor/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_motion_predictor.csv"); +- private_nh.param("/op_motion_predictor/rate", rate, 10); +- private_nh.param("/op_motion_predictor/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/op_motion_predictor/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/op_motion_predictor/task_relative_deadline", task_relative_deadline, (double)10); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_motion_predictor.csv"); + +- ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(25); +- +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(m_MapType == PlannerHNS::MAP_KML_FILE && !bMap) +- { +- bMap = true; +- PlannerHNS::MappingHelpers::LoadKML(m_MapPath, m_Map); +- } +- else if (m_MapType == PlannerHNS::MAP_FOLDER && !bMap) +- { +- bMap = true; +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromDataFiles(m_MapPath, m_Map, true); +- } +- else if (m_MapType == PlannerHNS::MAP_AUTOWARE && !bMap) +- { +- std::vector conn_data;; +- +- if(m_MapRaw.GetVersion()==2) +- { +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessageV2(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, +- m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, +- m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, +- m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, +- m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, +- m_MapRaw.pLanes, m_MapRaw.pPoints, m_MapRaw.pNodes, m_MapRaw.pLines, PlannerHNS::GPSPoint(), m_Map, true, m_PlanningParams.enableLaneChange, true); +- +- if(m_Map.roadSegments.size() > 0) +- { +- bMap = true; +- std::cout << " ******* Map V2 Is Loaded successfully from the Motion Predictor !! " << std::endl; +- } +- } +- else if(m_MapRaw.GetVersion()==1) +- { +- PlannerHNS::MappingHelpers::ConstructRoadNetworkFromROSMessage(m_MapRaw.pLanes->m_data_list, m_MapRaw.pPoints->m_data_list, +- m_MapRaw.pCenterLines->m_data_list, m_MapRaw.pIntersections->m_data_list,m_MapRaw.pAreas->m_data_list, +- m_MapRaw.pLines->m_data_list, m_MapRaw.pStopLines->m_data_list, m_MapRaw.pSignals->m_data_list, +- m_MapRaw.pVectors->m_data_list, m_MapRaw.pCurbs->m_data_list, m_MapRaw.pRoadedges->m_data_list, m_MapRaw.pWayAreas->m_data_list, +- m_MapRaw.pCrossWalks->m_data_list, m_MapRaw.pNodes->m_data_list, conn_data, PlannerHNS::GPSPoint(), m_Map, true); +- +- if(m_Map.roadSegments.size() > 0) +- { +- bMap = true; +- std::cout << " ******* Map V1 Is Loaded successfully from the Motion Predictor !! " << std::endl; +- } +- } +- } +- +- if(UtilityHNS::UtilityH::GetTimeDiffNow(m_VisualizationTimer) > m_VisualizationTime) +- { +- VisualizePrediction(); +- UtilityHNS::UtilityH::GetTickCount(m_VisualizationTimer); +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } ++ rubis::init_task_profiling(task_response_time_filename); + +- loop_rate.sleep(); +- } ++ ros::spin(); + } + + void MotionPrediction::TransformPose(const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped& out_pose, const tf::StampedTransform &in_transform) +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp +index d73353ad..5ff9be73 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp +@@ -30,6 +30,7 @@ TrajectoryEval::TrajectoryEval() + bWayGlobalPathToUse = false; + m_bUseMoveingObjectsPrediction = false; + m_noVehicleCnt = 0; ++ is_objects_updated_ = false; + + ros::NodeHandle _nh; + UpdatePlanningParams(_nh); +@@ -42,42 +43,29 @@ TrajectoryEval::TrajectoryEval() + + pub_CollisionPointsRviz = nh.advertise("dynamic_collision_points_rviz", 1); + pub_LocalWeightedTrajectoriesRviz = nh.advertise("local_trajectories_eval_rviz", 1); +- pub_LocalWeightedTrajectories = nh.advertise("local_weighted_trajectories", 1); ++ // pub_LocalWeightedTrajectories = nh.advertise("local_weighted_trajectories", 1); ++ pub_LocalWeightedTrajectoriesWithPoseTwist = nh.advertise("local_weighted_trajectories_with_pose_twist", 1); + pub_TrajectoryCost = nh.advertise("local_trajectory_cost", 1); + pub_SafetyBorderRviz = nh.advertise("safety_border", 1); + pub_DistanceToPedestrian = nh.advertise("distance_to_pedestrian", 1); + pub_IntersectionCondition = nh.advertise("intersection_condition", 1); + pub_SprintSwitch = nh.advertise("sprint_switch", 1); + +- sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this); +- sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this); +- +- int bVelSource = 1; +- _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); +- if(bVelSource == 0) +- sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this); +- else if(bVelSource == 1) +- sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this); +- else if(bVelSource == 2) +- sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this); +- +- /* RT Scheduling setup */ +- // sub_current_pose = nh.subscribe("/current_pose", 1, &TrajectoryEval::callbackGetCurrentPose, this); //origin 10 +- // sub_current_state = nh.subscribe("/current_state", 1, &TrajectoryEval::callbackGetCurrentState, this); //origin 10 ++ // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this); ++ // sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this); + + // int bVelSource = 1; + // _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); + // if(bVelSource == 0) +- // sub_robot_odom = nh.subscribe("/odom", 1, &TrajectoryEval::callbackGetRobotOdom, this); //origin 10 ++ // sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this); + // else if(bVelSource == 1) +- // sub_current_velocity = nh.subscribe("/current_velocity", 1, &TrajectoryEval::callbackGetVehicleStatus, this); //origin 10 ++ // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this); + // else if(bVelSource == 2) +- // sub_can_info = nh.subscribe("/can_info", 1, &TrajectoryEval::callbackGetCANInfo, this); //origin 10 ++ // sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this); + + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); +- sub_LocalPlannerPaths = nh.subscribe("/local_trajectories", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this); +- // sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); +- sub_rubis_predicted_objects = nh.subscribe("/rubis_predicted_objects", 1, &TrajectoryEval::callbackGetRubisPredictedObjects, this); ++ sub_LocalPlannerPaths = nh.subscribe("/local_trajectories_with_pose_twist", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this); ++ sub_predicted_objects = nh.subscribe("/rubis_predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); + sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this); + + PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy); +@@ -155,23 +143,21 @@ void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh) + + } + +-void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +-{ +- m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); +- bNewCurrentPos = true; +-} +- +-void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) +-{ +- m_VehicleStatus.speed = msg->twist.linear.x; +- m_CurrentPos.v = m_VehicleStatus.speed; +- if(fabs(msg->twist.linear.x) > 0.25) +- m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); +- UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); +- bVehicleStatus = true; +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +-} ++// void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) ++// { ++// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// bNewCurrentPos = true; ++// } ++ ++// void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) ++// { ++// m_VehicleStatus.speed = msg->twist.linear.x; ++// m_CurrentPos.v = m_VehicleStatus.speed; ++// if(fabs(msg->twist.linear.x) > 0.25) ++// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++// bVehicleStatus = true; ++// } + + void TrajectoryEval::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) + { +@@ -226,17 +212,51 @@ void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArray + } + } + +-void TrajectoryEval::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++void TrajectoryEval::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) + { +- if(msg->lanes.size() > 0) ++ rubis::start_task_profiling(); ++ // Before spin ++ UpdateMyParams(); ++ UpdateTf(); ++ ++ // callback for objects ++ if(is_objects_updated_){ ++ _callbackGetPredictedObjects(object_msg_); ++ is_objects_updated_ = false; ++ } ++ ++ static double prev_x = 0.0, prev_y = 0.0, prev_speed = 0.0; ++ ++ // callback for current pose ++ if(prev_x != msg->pose.pose.position.x || prev_y != msg->pose.pose.position.y){ ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ bNewCurrentPos = true; ++ prev_x = msg->pose.pose.position.x; ++ prev_y = msg->pose.pose.position.y; ++ } ++ ++ // callback for vehicle status ++ if(prev_speed != msg->twist.twist.linear.x){ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ prev_speed = msg->twist.twist.linear.x; ++ } ++ ++ ++ // callback for local planner path ++ if(msg->lane_array.lanes.size() > 0) + { + m_GeneratedRollOuts.clear(); + int globalPathId_roll_outs = -1; + +- for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) + { + std::vector path; +- PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), path); ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); + m_GeneratedRollOuts.push_back(path); + if(path.size() > 0) + globalPathId_roll_outs = path.at(0).gid; +@@ -257,157 +277,140 @@ void TrajectoryEval::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayC + + bRollOuts = true; + } +-} +- +-void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg) +-{ +- /* +- m_PredictedObjects.clear(); +- bPredictedObjects = true; +- double distance_to_pedestrian = 1000; +- int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2; +- int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2; +- +- int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2; +- int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2; + +- int vehicle_cnt = 0; ++ // After spin ++ PlannerHNS::TrajectoryCost tc; + +- PlannerHNS::DetectedObject obj; +- for(unsigned int i = 0 ; i objects.size(); i++) +- { +- if(msg->objects.at(i).pose.position.y < -20 || msg->objects.at(i).pose.position.y > 20) +- continue; +- +- if(msg->objects.at(i).pose.position.z > 1 || msg->objects.at(i).pose.position.z < -1.5) +- continue; ++ if(bNewCurrentPos && m_GlobalPaths.size()>0) ++ { ++ m_GlobalPathSections.clear(); + +- autoware_msgs::DetectedObject msg_obj = msg->objects.at(i); ++ for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++) ++ { ++ t_centerTrajectorySmoothed.clear(); ++ PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); ++ m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); ++ } + +- // #### Decison making for objects +- +- if(msg_obj.id > 0) // If fusion object is detected ++ if(m_GlobalPathSections.size()>0) + { +- if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ +- vehicle_cnt += 1; +- } ++ if(m_bUseMoveingObjectsPrediction) ++ tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory); ++ else ++ tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state); ++ ++ autoware_msgs::Lane l; ++ l.closest_object_distance = tc.closest_obj_distance; ++ l.closest_object_velocity = tc.closest_obj_velocity; ++ l.cost = tc.cost; ++ l.is_blocked = tc.bBlocked; ++ l.lane_index = tc.index; ++ pub_TrajectoryCost.publish(l); ++ ++ // hjw added : Check if ego is on intersection and obstacles are in risky area ++ int intersectionID = -1; ++ double closestIntersectionDistance = -1; ++ bool isInsideIntersection = false; ++ bool riskyLeftTurn = false; ++ bool riskyRightTurn = false; ++ ++ PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list_, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn); ++ ++ autoware_msgs::IntersectionCondition ic_msg; ++ ic_msg.intersectionID = intersectionID; ++ ic_msg.intersectionDistance = closestIntersectionDistance; ++ ic_msg.isIntersection = isInsideIntersection; ++ ic_msg.riskyLeftTurn = riskyLeftTurn; ++ ic_msg.riskyRightTurn = riskyRightTurn; ++ ++ pub_IntersectionCondition.publish(ic_msg); + +- PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj); ++ } + +- // transform center pose into map frame +- geometry_msgs::PoseStamped pose_in_map; +- pose_in_map.header = msg_obj.header; +- pose_in_map.pose = msg_obj.pose; +- try{ +- m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map); +- } +- catch(tf::TransformException& ex) ++ if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) ++ { ++ rubis_msgs::LaneArrayWithPoseTwist local_lanes; ++ for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++) + { +- // ROS_ERROR("Cannot transform object pose: %s", ex.what()); +- continue; ++ autoware_msgs::Lane lane; ++ PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane); ++ lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance; ++ lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity; ++ lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost; ++ lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked; ++ lane.lane_index = i; ++ local_lanes.lane_array.lanes.push_back(lane); + } +- // msg_obj.header.frame_id = "map"; +- obj.center.pos.x = pose_in_map.pose.position.x; +- obj.center.pos.y = pose_in_map.pose.position.y; +- obj.center.pos.z = pose_in_map.pose.position.z; +- +- // transform contour into map frame +- for(unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++){ +- geometry_msgs::PoseStamped contour_point_in_map; +- contour_point_in_map.header = msg_obj.header; +- contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x; +- contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y; +- contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z; +- +- // For resolve TF malform, set orientation w to 1 +- contour_point_in_map.pose.orientation.w = 1; + +- try{ +- m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map); +- } +- catch(tf::TransformException& ex){ +- // ROS_ERROR("Cannot transform contour pose: %s", ex.what()); +- continue; +- } ++ rubis::instance_ = msg->instance; ++ local_lanes.instance = rubis::instance_; ++ local_lanes.obj_instance = rubis::obj_instance_; ++ local_lanes.pose = msg->pose; ++ local_lanes.twist = msg->twist; + +- obj.contour.at(j).x = contour_point_in_map.pose.position.x; +- obj.contour.at(j).y = contour_point_in_map.pose.position.y; +- obj.contour.at(j).z = contour_point_in_map.pose.position.z; +- } ++ pub_LocalWeightedTrajectoriesWithPoseTwist.publish(local_lanes); ++ // pub_LocalWeightedTrajectories.publish(local_lanes.lane_array); ++ } ++ else ++ { ++ ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()"); ++ } + +- msg_obj.header.frame_id = "map"; ++ if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0) ++ { ++ visualization_msgs::MarkerArray all_rollOuts; ++ PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts); ++ pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts); + +- m_PredictedObjects.push_back(obj); +- } ++ PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy); ++ pub_CollisionPointsRviz.publish(m_CollisionsActual); + +- int image_obj_center_x = msg_obj.x+msg_obj.width/2; +- int image_obj_center_y = msg_obj.y+msg_obj.height/2; +- if (msg_obj.label == "person"){// If person is detected only in image +- +- // TO ERASE +- // ROS_WARN("object height:%d // thr: %d\n", msg_obj.height, m_pedestrian_stop_img_height_threshold); +- printf("center_x %d \n left: %d \n right %d\n\n\n", image_obj_center_x, image_person_detection_range_left, image_person_detection_range_right); +- if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ +- double temp_x_distance = 1000; +- if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; +- if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; +- } ++ //Visualize Safety Box ++ visualization_msgs::Marker safety_box; ++ PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box); ++ pub_SafetyBorderRviz.publish(safety_box); + } + } ++ else ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); + +- // Publish Sprint Switch +- std_msgs::Bool sprint_switch_msg; + +- if(vehicle_cnt != 0){ +- m_noVehicleCnt = 0; +- sprint_switch_msg.data = false; +- } +- else{ // No vehicle is exist in front of the car +- if(m_noVehicleCnt < m_SprintDecisionTime*10) { +- m_noVehicleCnt +=1; +- sprint_switch_msg.data = false; +- } +- else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true; +- } +- pub_SprintSwitch.publish(sprint_switch_msg); ++ rubis::stop_task_profiling(rubis::instance_, 0); ++} + +- std_msgs::Float64 distanceToPedestrianMsg; +- distanceToPedestrianMsg.data = distance_to_pedestrian; +- pub_DistanceToPedestrian.publish(distanceToPedestrianMsg); +- */ ++void TrajectoryEval::callbackGetPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg) ++{ ++ object_msg_ = msg->object_array; ++ rubis::obj_instance_ = msg->obj_instance; ++ is_objects_updated_ = true; + } + +-void TrajectoryEval::callbackGetRubisPredictedObjects(const rubis_msgs::DetectedObjectArrayConstPtr& msg) +-{ ++void TrajectoryEval::_callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArray& objects_msg){ + m_PredictedObjects.clear(); + bPredictedObjects = true; + double distance_to_pedestrian = 1000; + int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2; + int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2; +- + int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2; + int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2; +- + int vehicle_cnt = 0; + + PlannerHNS::DetectedObject obj; +- +- for(unsigned int i = 0 ; i object_array.objects.size(); i++) ++ for(unsigned int i = 0 ; i object_array.objects.at(i).pose.position.y < -20 || msg->object_array.objects.at(i).pose.position.y > 20) ++ if(objects_msg.objects.at(i).pose.position.y < -20 || objects_msg.objects.at(i).pose.position.y > 20) + continue; + +- if(msg->object_array.objects.at(i).pose.position.z > 1 || msg->object_array.objects.at(i).pose.position.z < -1.5) ++ if(objects_msg.objects.at(i).pose.position.z > 1 || objects_msg.objects.at(i).pose.position.z < -1.5) + continue; + +- autoware_msgs::DetectedObject msg_obj = msg->object_array.objects.at(i); ++ autoware_msgs::DetectedObject msg_obj = objects_msg.objects.at(i); + + if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ + vehicle_cnt += 1; + } + +- PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->object_array.objects.at(i), obj); +- ++ PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(objects_msg.objects.at(i), obj); + geometry_msgs::PoseStamped pose_in_map; + pose_in_map.header = msg_obj.header; + pose_in_map.pose = msg_obj.pose; +@@ -468,7 +471,6 @@ void TrajectoryEval::callbackGetRubisPredictedObjects(const rubis_msgs::Detected + if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; + } + } +- + } + + // Publish Sprint Switch +@@ -485,6 +487,7 @@ void TrajectoryEval::callbackGetRubisPredictedObjects(const rubis_msgs::Detected + } + else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true; + } ++ + pub_SprintSwitch.publish(sprint_switch_msg); + + std_msgs::Float64 distanceToPedestrianMsg; +@@ -510,6 +513,11 @@ void TrajectoryEval::UpdateMyParams() + _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong); + _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat); + _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance); ++ ++ _nh.getParam("/op_trajectory_evaluator/lateralBlockingThreshold", m_PlanningParams.lateralBlockingThreshold); ++ _nh.getParam("/op_trajectory_evaluator/frontLongitudinalBlockingThreshold", m_PlanningParams.frontLongitudinalBlockingThreshold); ++ _nh.getParam("/op_trajectory_evaluator/rearLongitudinalBlockingThreshold", m_PlanningParams.rearLongitudinalBlockingThreshold); ++ _nh.getParam("/op_trajectory_evaluator/enableDebug", m_PlanningParams.enableDebug); + } + + bool TrajectoryEval::UpdateTf() +@@ -533,150 +541,35 @@ void TrajectoryEval::MainLoop() + { + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"); ++ + int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; ++ private_nh.param(node_name+"/rate", rate, 10); + +- private_nh.param("/op_trajectory_evaluator/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_trajectory_evaluator/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/op_trajectory_evaluator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"); +- private_nh.param("/op_trajectory_evaluator/rate", rate, 10); +- private_nh.param("/op_trajectory_evaluator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/op_trajectory_evaluator/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/op_trajectory_evaluator/task_relative_deadline", task_relative_deadline, (double)10); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + PlannerHNS::WayPoint prevState, state_change; + + // Add Crossing Info from yaml file +- XmlRpc::XmlRpcValue intersection_xml; +- std::vector intersection_list; ++ XmlRpc::XmlRpcValue intersection_xml; + nh.getParam("/op_trajectory_evaluator/intersection_list", intersection_xml); +- PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list, intersection_xml); +- +- +- +- ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(100); ++ PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list_, intersection_xml); + +- struct timespec start_time, end_time; +- +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- UpdateMyParams(); +- UpdateTf(); +- +- ros::spinOnce(); +- PlannerHNS::TrajectoryCost tc; +- +- if(bNewCurrentPos && m_GlobalPaths.size()>0) +- { +- m_GlobalPathSections.clear(); +- +- for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++) +- { +- t_centerTrajectorySmoothed.clear(); +- PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); +- m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); +- } +- +- if(m_GlobalPathSections.size()>0) +- { +- if(m_bUseMoveingObjectsPrediction) +- tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory); +- else +- tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state); +- +- autoware_msgs::Lane l; +- l.closest_object_distance = tc.closest_obj_distance; +- l.closest_object_velocity = tc.closest_obj_velocity; +- l.cost = tc.cost; +- l.is_blocked = tc.bBlocked; +- l.lane_index = tc.index; +- pub_TrajectoryCost.publish(l); +- +- // hjw added : Check if ego is on intersection and obstacles are in risky area +- int intersectionID = -1; +- double closestIntersectionDistance = -1; +- bool isInsideIntersection = false; +- bool riskyLeftTurn = false; +- bool riskyRightTurn = false; +- +- PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn); +- +- autoware_msgs::IntersectionCondition ic_msg; +- ic_msg.intersectionID = intersectionID; +- ic_msg.intersectionDistance = closestIntersectionDistance; +- ic_msg.isIntersection = isInsideIntersection; +- ic_msg.riskyLeftTurn = riskyLeftTurn; +- ic_msg.riskyRightTurn = riskyRightTurn; +- +- pub_IntersectionCondition.publish(ic_msg); +- +- } +- +- if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) +- { +- autoware_msgs::LaneArray local_lanes; +- for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++) +- { +- autoware_msgs::Lane lane; +- PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane); +- lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance; +- lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity; +- lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost; +- lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked; +- lane.lane_index = i; +- local_lanes.lanes.push_back(lane); +- } +- +- pub_LocalWeightedTrajectories.publish(local_lanes); +- rubis::sched::task_state_ = TASK_STATE_DONE; +- } +- else +- { +- ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()"); +- } +- +- if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0) +- { +- visualization_msgs::MarkerArray all_rollOuts; +- PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts); +- pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts); +- +- PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy); +- pub_CollisionPointsRviz.publish(m_CollisionsActual); +- +- //Visualize Safety Box +- visualization_msgs::Marker safety_box; +- PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box); +- pub_SafetyBorderRviz.publish(safety_box); +- } +- } +- else +- sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- loop_rate.sleep(); +- } ++ ros::spin(); + } +- + } +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp +new file mode 100644 +index 00000000..a1ce5e83 +--- /dev/null ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.mainloop.cpp +@@ -0,0 +1,596 @@ ++/* ++ * Copyright 2018-2019 Autoware Foundation. All rights reserved. ++ * ++ * Licensed under the Apache License, Version 2.0 (the "License"); ++ * you may not use this file except in compliance with the License. ++ * You may obtain a copy of the License at ++ * ++ * http://www.apache.org/licenses/LICENSE-2.0 ++ * ++ * Unless required by applicable law or agreed to in writing, software ++ * distributed under the License is distributed on an "AS IS" BASIS, ++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ++ * See the License for the specific language governing permissions and ++ * limitations under the License. ++ */ ++ ++#include "op_trajectory_evaluator_core.h" ++#include "op_ros_helpers/op_ROSHelpers.h" ++#include "op_planner/MappingHelpers.h" ++#include ++ ++namespace TrajectoryEvaluatorNS ++{ ++ ++TrajectoryEval::TrajectoryEval() ++{ ++ bNewCurrentPos = false; ++ bVehicleStatus = false; ++ bWayGlobalPath = false; ++ bWayGlobalPathToUse = false; ++ m_bUseMoveingObjectsPrediction = false; ++ m_noVehicleCnt = 0; ++ ++ ros::NodeHandle _nh; ++ UpdatePlanningParams(_nh); ++ ++ tf::StampedTransform transform; ++ PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); ++ m_OriginPos.position.x = transform.getOrigin().x(); ++ m_OriginPos.position.y = transform.getOrigin().y(); ++ m_OriginPos.position.z = transform.getOrigin().z(); ++ ++ pub_CollisionPointsRviz = nh.advertise("dynamic_collision_points_rviz", 1); ++ pub_LocalWeightedTrajectoriesRviz = nh.advertise("local_trajectories_eval_rviz", 1); ++ pub_LocalWeightedTrajectories = nh.advertise("local_weighted_trajectories", 1); ++ pub_LocalWeightedTrajectoriesWithPoseTwist = nh.advertise("local_weighted_trajectories_with_pose_twist", 1); ++ pub_TrajectoryCost = nh.advertise("local_trajectory_cost", 1); ++ pub_SafetyBorderRviz = nh.advertise("safety_border", 1); ++ pub_DistanceToPedestrian = nh.advertise("distance_to_pedestrian", 1); ++ pub_IntersectionCondition = nh.advertise("intersection_condition", 1); ++ pub_SprintSwitch = nh.advertise("sprint_switch", 1); ++ ++ // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryEval::callbackGetCurrentPose, this); ++ // sub_current_state = nh.subscribe("/current_state", 10, &TrajectoryEval::callbackGetCurrentState, this); ++ ++ int bVelSource = 1; ++ _nh.getParam("/op_trajectory_evaluator/velocitySource", bVelSource); ++ if(bVelSource == 0) ++ sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryEval::callbackGetRobotOdom, this); ++ // else if(bVelSource == 1) ++ // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryEval::callbackGetVehicleStatus, this); ++ else if(bVelSource == 2) ++ sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryEval::callbackGetCANInfo, this); ++ ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); ++ sub_LocalPlannerPaths = nh.subscribe("/local_trajectories_with_pose_twist", 1, &TrajectoryEval::callbackGetLocalPlannerPath, this); ++ sub_predicted_objects = nh.subscribe("/predicted_objects", 1, &TrajectoryEval::callbackGetPredictedObjects, this); ++ sub_current_behavior = nh.subscribe("/current_behavior", 1, &TrajectoryEval::callbackGetBehaviorState, this); ++ ++ PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(50, m_CollisionsDummy); ++ ++ while(1){ ++ if(UpdateTf() == true) ++ break; ++ } ++} ++ ++TrajectoryEval::~TrajectoryEval() ++{ ++} ++ ++void TrajectoryEval::UpdatePlanningParams(ros::NodeHandle& _nh) ++{ ++ _nh.getParam("/op_trajectory_evaluator/enablePrediction", m_bUseMoveingObjectsPrediction); ++ ++ _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); ++ _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); ++ _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); ++ if(m_PlanningParams.enableSwerving) ++ m_PlanningParams.enableFollowing = true; ++ else ++ _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); ++ ++ _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); ++ _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); ++ ++ _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); ++ _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); ++ _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); ++ ++ _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); ++ ++ _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); ++ if(m_PlanningParams.enableSwerving) ++ _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); ++ else ++ m_PlanningParams.rollOutNumber = 0; ++ ++ std::cout << "Rolls Number: " << m_PlanningParams.rollOutNumber << std::endl; ++ ++ _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); ++ _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); ++ _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); ++ _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); ++ _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); ++ ++ _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); ++ ++ _nh.getParam("/op_common_params/width", m_CarInfo.width); ++ _nh.getParam("/op_common_params/length", m_CarInfo.length); ++ _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); ++ _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); ++ _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); ++ _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); ++ _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); ++ m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; ++ m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; ++ ++ _nh.param("/op_trajectory_evaluator/PedestrianRightThreshold", m_PedestrianRightThreshold, 7.0); ++ _nh.param("/op_trajectory_evaluator/PedestrianLeftThreshold", m_PedestrianLeftThreshold, 2.0); ++ _nh.param("/op_trajectory_evaluator/PedestrianImageDetectionRange", m_PedestrianImageDetectionRange, 0.7); ++ _nh.param("/op_trajectory_evaluator/PedestrianStopImgHeightThreshold", m_pedestrian_stop_img_height_threshold, 120); ++ _nh.param("/op_trajectory_evaluator/ImageWidth", m_ImageWidth, 1920); ++ _nh.param("/op_trajectory_evaluator/ImageHeight", m_ImageHeight, 1080); ++ _nh.param("/op_trajectory_evaluator/VehicleImageDetectionRange", m_VehicleImageDetectionRange, 0.3); ++ _nh.param("/op_trajectory_evaluator/VehicleImageWidthThreshold", m_VehicleImageWidthThreshold, 0.05); ++ _nh.param("/op_trajectory_evaluator/SprintDecisionTime", m_SprintDecisionTime, 5.0); ++ ++ ++ m_VehicleImageWidthThreshold = m_VehicleImageWidthThreshold * m_ImageWidth; ++ m_PedestrianRightThreshold *= -1; ++ ++} ++ ++// void TrajectoryEval::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) ++// { ++// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// bNewCurrentPos = true; ++// } ++ ++// void TrajectoryEval::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) ++// { ++// m_VehicleStatus.speed = msg->twist.linear.x; ++// m_CurrentPos.v = m_VehicleStatus.speed; ++// if(fabs(msg->twist.linear.x) > 0.25) ++// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++// bVehicleStatus = true; ++// } ++ ++void TrajectoryEval::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) ++{ ++ m_VehicleStatus.speed = msg->speed/3.6; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void TrajectoryEval::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) ++{ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void TrajectoryEval::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++{ ++ if(msg->lanes.size() > 0) ++ { ++ ++ bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); ++ ++ m_GlobalPaths.clear(); ++ ++ for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ { ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); ++ ++ PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); ++ m_GlobalPaths.push_back(m_temp_path); ++ ++ if(bOldGlobalPath) ++ { ++ bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); ++ } ++ } ++ ++ if(!bOldGlobalPath) ++ { ++ bWayGlobalPath = true; ++ std::cout << "Received New Global Path Evaluator! " << std::endl; ++ } ++ else ++ { ++ m_GlobalPaths.clear(); ++ } ++ } ++} ++ ++void TrajectoryEval::callbackGetLocalPlannerPath(const rubis_msgs::LaneArrayWithPoseTwistConstPtr& msg) ++{ ++ rubis::instance_ = msg->instance; ++ // Callback ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ bNewCurrentPos = true; ++ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ ++ if(msg->lane_array.lanes.size() > 0) ++ { ++ m_GeneratedRollOuts.clear(); ++ int globalPathId_roll_outs = -1; ++ ++ for(unsigned int i = 0 ; i < msg->lane_array.lanes.size(); i++) ++ { ++ std::vector path; ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lane_array.lanes.at(i), path); ++ m_GeneratedRollOuts.push_back(path); ++ if(path.size() > 0) ++ globalPathId_roll_outs = path.at(0).gid; ++ } ++ ++ if(bWayGlobalPath && m_GlobalPaths.size() > 0 && m_GlobalPaths.at(0).size() > 0) ++ { ++ int globalPathId = m_GlobalPaths.at(0).at(0).gid; ++ std::cout << "Before Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; ++ ++ if(globalPathId_roll_outs == globalPathId) ++ { ++ bWayGlobalPath = false; ++ m_GlobalPathsToUse = m_GlobalPaths; ++ std::cout << "Synchronization At Trajectory Evaluator: GlobalID: " << globalPathId << ", LocalID: " << globalPathId_roll_outs << std::endl; ++ } ++ } ++ ++ bRollOuts = true; ++ } ++} ++ ++void TrajectoryEval::callbackGetPredictedObjects(const autoware_msgs::DetectedObjectArrayConstPtr& msg) ++{ ++ m_PredictedObjects.clear(); ++ bPredictedObjects = true; ++ double distance_to_pedestrian = 1000; ++ int image_person_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_PedestrianImageDetectionRange/2; ++ int image_person_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_PedestrianImageDetectionRange/2; ++ ++ int image_vehicle_detection_range_left = m_ImageWidth/2 - m_ImageWidth*m_VehicleImageDetectionRange/2; ++ int image_vehicle_detection_range_right = m_ImageWidth/2 + m_ImageWidth*m_VehicleImageDetectionRange/2; ++ ++ int vehicle_cnt = 0; ++ ++ PlannerHNS::DetectedObject obj; ++ for(unsigned int i = 0 ; i objects.size(); i++) ++ { ++ if(msg->objects.at(i).pose.position.y < -20 || msg->objects.at(i).pose.position.y > 20) ++ continue; ++ ++ if(msg->objects.at(i).pose.position.z > 1 || msg->objects.at(i).pose.position.z < -1.5) ++ continue; ++ ++ autoware_msgs::DetectedObject msg_obj = msg->objects.at(i); ++ ++ // #### Decison making for objects ++ ++ if(msg_obj.id > 0) // If fusion object is detected ++ { ++ // calculate distance to person first ++ // if(msg_obj.label == "person"){ ++ // std::cout<<"Pedestrian box size(width x height):"< m_PedestrianLeftThreshold || temp_y_distance < m_PedestrianRightThreshold ) continue; ++ // if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; ++ // } ++ // catch(tf::TransformException& ex){ ++ // // ROS_ERROR("Cannot transform person pose: %s", ex.what()); ++ ++ // } ++ // } ++ ++ if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ ++ vehicle_cnt += 1; ++ } ++ ++ PlannerHNS::ROSHelpers::ConvertFromAutowareDetectedObjectToOpenPlannerDetectedObject(msg->objects.at(i), obj); ++ ++ ++ // transform center pose into map frame ++ geometry_msgs::PoseStamped pose_in_map; ++ pose_in_map.header = msg_obj.header; ++ pose_in_map.pose = msg_obj.pose; ++ try{ ++ m_vtom_listener.transformPose("/map", pose_in_map, pose_in_map); ++ } ++ catch(tf::TransformException& ex) ++ { ++ // ROS_ERROR("Cannot transform object pose: %s", ex.what()); ++ continue; ++ } ++ // msg_obj.header.frame_id = "map"; ++ obj.center.pos.x = pose_in_map.pose.position.x; ++ obj.center.pos.y = pose_in_map.pose.position.y; ++ obj.center.pos.z = pose_in_map.pose.position.z; ++ ++ // transform contour into map frame ++ for(unsigned int j = 0; j < msg_obj.convex_hull.polygon.points.size(); j++){ ++ geometry_msgs::PoseStamped contour_point_in_map; ++ contour_point_in_map.header = msg_obj.header; ++ contour_point_in_map.pose.position.x = msg_obj.convex_hull.polygon.points.at(j).x; ++ contour_point_in_map.pose.position.y = msg_obj.convex_hull.polygon.points.at(j).y; ++ contour_point_in_map.pose.position.z = msg_obj.convex_hull.polygon.points.at(j).z; ++ ++ // For resolve TF malform, set orientation w to 1 ++ contour_point_in_map.pose.orientation.w = 1; ++ ++ try{ ++ m_vtom_listener.transformPose("/map", contour_point_in_map, contour_point_in_map); ++ } ++ catch(tf::TransformException& ex){ ++ // ROS_ERROR("Cannot transform contour pose: %s", ex.what()); ++ continue; ++ } ++ ++ obj.contour.at(j).x = contour_point_in_map.pose.position.x; ++ obj.contour.at(j).y = contour_point_in_map.pose.position.y; ++ obj.contour.at(j).z = contour_point_in_map.pose.position.z; ++ } ++ ++ msg_obj.header.frame_id = "map"; ++ ++ m_PredictedObjects.push_back(obj); ++ } ++ /* ++ else{ // If object is only detected at vision ++ int image_obj_center_x = msg_obj.x+msg_obj.width/2; ++ int image_obj_center_y = msg_obj.y+msg_obj.height/2; ++ // if (msg_obj.label == "person"){// If person is detected only in image ++ // // TO ERASE ++ // std::cout<<"object height:" << msg_obj.height << " / threshold:" << m_pedestrian_stop_img_height_threshold << std::endl; ++ // if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ ++ // double temp_x_distance = 1000; ++ // if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; ++ // if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; ++ // } ++ // } ++ // else ++ if(msg_obj.label == "car" || msg_obj.label == "truck" || msg_obj.label == "bus"){ ++ if((msg_obj.width > m_VehicleImageWidthThreshold) ++ && (image_obj_center_x > image_vehicle_detection_range_left) ++ && (image_obj_center_x < image_vehicle_detection_range_right) ++ ) ++ { ++ vehicle_cnt+=1; ++ } ++ } ++ } ++ */ ++ ++ int image_obj_center_x = msg_obj.x+msg_obj.width/2; ++ int image_obj_center_y = msg_obj.y+msg_obj.height/2; ++ if (msg_obj.label == "person"){// If person is detected only in image ++ ++ // TO ERASE ++ // ROS_WARN("object height:%d // thr: %d\n", msg_obj.height, m_pedestrian_stop_img_height_threshold); ++ printf("center_x %d \n left: %d \n right %d\n\n\n", image_obj_center_x, image_person_detection_range_left, image_person_detection_range_right); ++ if(image_obj_center_x >= image_person_detection_range_left && image_obj_center_x <= image_person_detection_range_right){ ++ double temp_x_distance = 1000; ++ if(msg_obj.height >= m_pedestrian_stop_img_height_threshold) temp_x_distance = 10; ++ if(abs(temp_x_distance) < abs(distance_to_pedestrian)) distance_to_pedestrian = temp_x_distance; ++ } ++ } ++ ++ } ++ ++ // Publish Sprint Switch ++ std_msgs::Bool sprint_switch_msg; ++ ++ if(vehicle_cnt != 0){ ++ m_noVehicleCnt = 0; ++ sprint_switch_msg.data = false; ++ } ++ else{ // No vehicle is exist in front of the car ++ if(m_noVehicleCnt < m_SprintDecisionTime*10) { ++ m_noVehicleCnt +=1; ++ sprint_switch_msg.data = false; ++ } ++ else if (m_noVehicleCnt >= 5) sprint_switch_msg.data = true; ++ } ++ pub_SprintSwitch.publish(sprint_switch_msg); ++ ++ // ROS_INFO("object # : %d", m_PredictedObjects.size()); ++ ++ std_msgs::Float64 distanceToPedestrianMsg; ++ distanceToPedestrianMsg.data = distance_to_pedestrian; ++ pub_DistanceToPedestrian.publish(distanceToPedestrianMsg); ++} ++ ++void TrajectoryEval::callbackGetBehaviorState(const geometry_msgs::TwistStampedConstPtr& msg) ++{ ++ m_CurrentBehavior.iTrajectory = msg->twist.angular.z; ++} ++ ++void TrajectoryEval::callbackGetCurrentState(const std_msgs::Int32 & msg) ++{ ++ m_CurrentBehavior.state = static_cast(msg.data); ++} ++ ++void TrajectoryEval::UpdateMyParams() ++{ ++ ros::NodeHandle _nh; ++ _nh.getParam("/op_trajectory_evaluator/weightPriority", m_PlanningParams.weightPriority); ++ _nh.getParam("/op_trajectory_evaluator/weightTransition", m_PlanningParams.weightTransition); ++ _nh.getParam("/op_trajectory_evaluator/weightLong", m_PlanningParams.weightLong); ++ _nh.getParam("/op_trajectory_evaluator/weightLat", m_PlanningParams.weightLat); ++ _nh.getParam("/op_trajectory_evaluator/LateralSkipDistance", m_PlanningParams.LateralSkipDistance); ++} ++ ++bool TrajectoryEval::UpdateTf() ++{ ++ try{ ++ m_vtob_listener.waitForTransform("/velodyne", "/base_link", ros::Time(0), ros::Duration(0.001)); ++ m_vtob_listener.lookupTransform("/velodyne", "/base_link", ros::Time(0), m_velodyne_to_base_link); ++ ++ m_vtom_listener.waitForTransform("/velodyne", "/map", ros::Time(0), ros::Duration(0.001)); ++ m_vtom_listener.lookupTransform("/velodyne", "/map", ros::Time(0), m_velodyne_to_map); ++ return true; ++ } ++ catch(tf::TransformException& ex){ ++ if(TF_DEBUG) ++ ROS_ERROR("%s", ex.what()); ++ return false; ++ } ++} ++ ++void TrajectoryEval::MainLoop() ++{ ++ ros::NodeHandle private_nh("~"); ++ ++ // Scheduling Setup ++ std::string task_response_time_filename; ++ int rate; ++ double task_minimum_inter_release_time; ++ double task_execution_time; ++ double task_relative_deadline; ++ ++ private_nh.param("/op_trajectory_evaluator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_evaluator.csv"); ++ private_nh.param("/op_trajectory_evaluator/rate", rate, 10); ++ private_nh.param("/op_trajectory_evaluator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); ++ private_nh.param("/op_trajectory_evaluator/task_execution_time", task_execution_time, (double)10); ++ private_nh.param("/op_trajectory_evaluator/task_relative_deadline", task_relative_deadline, (double)10); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ PlannerHNS::WayPoint prevState, state_change; ++ ++ // Add Crossing Info from yaml file ++ XmlRpc::XmlRpcValue intersection_xml; ++ nh.getParam("/op_trajectory_evaluator/intersection_list", intersection_xml); ++ PlannerHNS::MappingHelpers::ConstructIntersection_RUBIS(intersection_list_, intersection_xml); ++ ++ ros::Rate r(100); ++ ++ while(ros::ok()){ ++ // Before spin ++ UpdateMyParams(); ++ UpdateTf(); ++ ++ ros::spinOnce(); ++ ++ PlannerHNS::TrajectoryCost tc; ++ ++ if(bNewCurrentPos && m_GlobalPaths.size()>0) ++ { ++ m_GlobalPathSections.clear(); ++ ++ for(unsigned int i = 0; i < m_GlobalPathsToUse.size(); i++) ++ { ++ t_centerTrajectorySmoothed.clear(); ++ PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPathsToUse.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); ++ m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); ++ } ++ ++ if(m_GlobalPathSections.size()>0) ++ { ++ if(m_bUseMoveingObjectsPrediction) ++ tc = m_TrajectoryCostsCalculator.DoOneStepDynamic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos,m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.iTrajectory); ++ else ++ tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_GeneratedRollOuts, m_GlobalPathSections.at(0), m_CurrentPos, m_PlanningParams, m_CarInfo,m_VehicleStatus, m_PredictedObjects, m_CurrentBehavior.state); ++ ++ autoware_msgs::Lane l; ++ l.closest_object_distance = tc.closest_obj_distance; ++ l.closest_object_velocity = tc.closest_obj_velocity; ++ l.cost = tc.cost; ++ l.is_blocked = tc.bBlocked; ++ l.lane_index = tc.index; ++ pub_TrajectoryCost.publish(l); ++ ++ // hjw added : Check if ego is on intersection and obstacles are in risky area ++ int intersectionID = -1; ++ double closestIntersectionDistance = -1; ++ bool isInsideIntersection = false; ++ bool riskyLeftTurn = false; ++ bool riskyRightTurn = false; ++ ++ PlannerHNS::PlanningHelpers::GetIntersectionCondition(m_CurrentPos, intersection_list_, m_PredictedObjects, intersectionID, closestIntersectionDistance, isInsideIntersection, riskyLeftTurn, riskyRightTurn); ++ ++ autoware_msgs::IntersectionCondition ic_msg; ++ ic_msg.intersectionID = intersectionID; ++ ic_msg.intersectionDistance = closestIntersectionDistance; ++ ic_msg.isIntersection = isInsideIntersection; ++ ic_msg.riskyLeftTurn = riskyLeftTurn; ++ ic_msg.riskyRightTurn = riskyRightTurn; ++ ++ pub_IntersectionCondition.publish(ic_msg); ++ ++ } ++ ++ if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size() == m_GeneratedRollOuts.size()) ++ { ++ rubis_msgs::LaneArrayWithPoseTwist local_lanes; ++ for(unsigned int i=0; i < m_GeneratedRollOuts.size(); i++) ++ { ++ autoware_msgs::Lane lane; ++ PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_GeneratedRollOuts.at(i), lane); ++ lane.closest_object_distance = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_distance; ++ lane.closest_object_velocity = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).closest_obj_velocity; ++ lane.cost = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).cost; ++ lane.is_blocked = m_TrajectoryCostsCalculator.m_TrajectoryCosts.at(i).bBlocked; ++ lane.lane_index = i; ++ local_lanes.lane_array.lanes.push_back(lane); ++ } ++ ++ local_lanes.instance = rubis::instance_; ++ local_lanes.pose = msg->pose; ++ local_lanes.twist = msg->twist; ++ ++ pub_LocalWeightedTrajectoriesWithPoseTwist.publish(local_lanes); ++ pub_LocalWeightedTrajectories.publish(local_lanes.lane_array); ++ ++ } ++ else ++ { ++ ROS_ERROR("m_TrajectoryCosts.size() Not Equal m_GeneratedRollOuts.size()"); ++ } ++ ++ if(m_TrajectoryCostsCalculator.m_TrajectoryCosts.size()>0) ++ { ++ visualization_msgs::MarkerArray all_rollOuts; ++ PlannerHNS::ROSHelpers::TrajectoriesToColoredMarkers(m_GeneratedRollOuts, m_TrajectoryCostsCalculator.m_TrajectoryCosts, m_CurrentBehavior.iTrajectory, all_rollOuts); ++ pub_LocalWeightedTrajectoriesRviz.publish(all_rollOuts); ++ ++ PlannerHNS::ROSHelpers::ConvertCollisionPointsMarkers(m_TrajectoryCostsCalculator.m_CollisionPoints, m_CollisionsActual, m_CollisionsDummy); ++ pub_CollisionPointsRviz.publish(m_CollisionsActual); ++ ++ //Visualize Safety Box ++ visualization_msgs::Marker safety_box; ++ PlannerHNS::ROSHelpers::ConvertFromPlannerHRectangleToAutowareRviz(m_TrajectoryCostsCalculator.m_SafetyBorder.points, safety_box); ++ pub_SafetyBorderRviz.publish(safety_box); ++ } ++ } ++ else ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryEval::callbackGetGlobalPlannerPath, this); ++ ++ rubis::stop_task_profiling(0, 0); ++ ++ r.sleep(); ++ } ++} ++ ++} +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp +index 10549a37..be9bebe5 100644 +--- a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.cpp +@@ -39,37 +39,24 @@ TrajectoryGen::TrajectoryGen() + m_OriginPos.position.y = transform.getOrigin().y(); + m_OriginPos.position.z = transform.getOrigin().z(); + +- pub_LocalTrajectories = nh.advertise("local_trajectories", 1); ++ // pub_LocalTrajectories = nh.advertise("local_trajectories", 1); ++ pub_LocalTrajectoriesWithPoseTwist = nh.advertise("local_trajectories_with_pose_twist", 1); + pub_LocalTrajectoriesRviz = nh.advertise("local_trajectories_gen_rviz", 1); + + sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this); +- sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryGen::callbackGetCurrentPose, this); + + int bVelSource = 1; + _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource); +- if(bVelSource == 0) +- sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryGen::callbackGetRobotOdom, this); +- else if(bVelSource == 1) +- sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this); +- else if(bVelSource == 2) +- sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this); +- +- sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); +- +- /* RT Scheduling setup */ +- // sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this); +- // sub_current_pose = nh.subscribe("/current_pose", 1, &TrajectoryGen::callbackGetCurrentPose, this); //origin 10 +- +- // int bVelSource = 1; +- // _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource); + // if(bVelSource == 0) +- // sub_robot_odom = nh.subscribe("/odom", 1, &TrajectoryGen::callbackGetRobotOdom, this); //origin 10 ++ // sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryGen::callbackGetRobotOdom, this); + // else if(bVelSource == 1) +- // sub_current_velocity = nh.subscribe("/current_velocity", 1, &TrajectoryGen::callbackGetVehicleStatus, this); //origin 10 ++ // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this); + // else if(bVelSource == 2) +- // sub_can_info = nh.subscribe("/can_info", 1, &TrajectoryGen::callbackGetCANInfo, this); //origin 10 ++ // sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this); + +- // sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); ++ ++ sub_pose_twist = nh.subscribe("/rubis_current_pose_twist", 1, &TrajectoryGen::callbackGetCurrentPoseTwist, this); // Def: 10 + } + + TrajectoryGen::~TrajectoryGen() +@@ -143,26 +130,45 @@ void TrajectoryGen::callbackGetInitPose(const geometry_msgs::PoseWithCovarianceS + } + } + +-void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +-{ +- m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) ++// { ++// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// m_InitPos = m_CurrentPos; ++// bNewCurrentPos = true; ++// bInitPos = true; ++// } ++ ++// void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) ++// { ++// m_VehicleStatus.speed = msg->twist.linear.x; ++// m_CurrentPos.v = m_VehicleStatus.speed; ++// if(fabs(msg->twist.linear.x) > 0.25) ++// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++// bVehicleStatus = true; ++// } ++ ++void TrajectoryGen::callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg){ ++ // Callback ++ rubis::instance_ = msg->instance; ++ ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); + m_InitPos = m_CurrentPos; + bNewCurrentPos = true; + bInitPos = true; +-} + +-void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) +-{ +- m_VehicleStatus.speed = msg->twist.linear.x; ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; + m_CurrentPos.v = m_VehicleStatus.speed; +- if(fabs(msg->twist.linear.x) > 0.25) +- m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); + UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); + bVehicleStatus = true; + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); ++ current_pose_ = msg->pose; ++ current_twist_ = msg->twist; + } + ++ + void TrajectoryGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) + { + m_VehicleStatus.speed = msg->speed/3.6; +@@ -216,41 +222,34 @@ void TrajectoryGen::MainLoop() + { + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv"); + +- private_nh.param("/op_trajectory_generator/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/op_trajectory_generator/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/op_trajectory_generator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv"); +- private_nh.param("/op_trajectory_generator/rate", rate, 10); +- private_nh.param("/op_trajectory_generator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/op_trajectory_generator/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/op_trajectory_generator/task_relative_deadline", task_relative_deadline, (double)10); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); + + PlannerHNS::WayPoint prevState, state_change; + ++ ros::Rate r(rate); + +- ros::Rate loop_rate(rate); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(100); +- +- struct timespec start_time, end_time; +- +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ while(ros::ok()){ ++ rubis::start_task_profiling(); + + ros::spinOnce(); + +@@ -288,7 +287,7 @@ void TrajectoryGen::MainLoop() + -1 , -1, + m_RollOuts, sampledPoints_debug); + +- autoware_msgs::LaneArray local_lanes; ++ rubis_msgs::LaneArrayWithPoseTwist local_lanes; + for(unsigned int i=0; i < m_RollOuts.size(); i++) + { + for(unsigned int j=0; j < m_RollOuts.at(i).size(); j++) +@@ -301,28 +300,30 @@ void TrajectoryGen::MainLoop() + lane.cost = 0; + lane.is_blocked = false; + lane.lane_index = i; +- local_lanes.lanes.push_back(lane); ++ local_lanes.lane_array.lanes.push_back(lane); + } + } +- pub_LocalTrajectories.publish(local_lanes); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ ++ local_lanes.instance = rubis::instance_; ++ local_lanes.pose = current_pose_; ++ local_lanes.twist = current_twist_; ++ ++ pub_LocalTrajectoriesWithPoseTwist.publish(local_lanes); ++ // pub_LocalTrajectories.publish(local_lanes.lane_array); ++ + } +- else ++ else{ + sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); + +- visualization_msgs::MarkerArray all_rollOuts; +- PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts); +- pub_LocalTrajectoriesRviz.publish(all_rollOuts); ++ visualization_msgs::MarkerArray all_rollOuts; ++ PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts); ++ pub_LocalTrajectoriesRviz.publish(all_rollOuts); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; ++ rubis::stop_task_profiling(0, 0); + } + +- loop_rate.sleep(); +- } ++ r.sleep(); ++ } + } + +-} ++} +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp +new file mode 100644 +index 00000000..2c40bb81 +--- /dev/null ++++ b/autoware.ai/src/autoware/core_planning/op_local_planner/nodes/op_trajectory_generator/op_trajectory_generator_core.modified.cpp +@@ -0,0 +1,315 @@ ++/* ++ * Copyright 2018-2019 Autoware Foundation. All rights reserved. ++ * ++ * Licensed under the Apache License, Version 2.0 (the "License"); ++ * you may not use this file except in compliance with the License. ++ * You may obtain a copy of the License at ++ * ++ * http://www.apache.org/licenses/LICENSE-2.0 ++ * ++ * Unless required by applicable law or agreed to in writing, software ++ * distributed under the License is distributed on an "AS IS" BASIS, ++ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ++ * See the License for the specific language governing permissions and ++ * limitations under the License. ++ */ ++ ++#include "op_trajectory_generator_core.h" ++#include "op_ros_helpers/op_ROSHelpers.h" ++#include ++ ++#define SPIN_PROFILING ++ ++namespace TrajectoryGeneratorNS ++{ ++ ++TrajectoryGen::TrajectoryGen() ++{ ++ bInitPos = false; ++ bNewCurrentPos = false; ++ bVehicleStatus = false; ++ bWayGlobalPath = false; ++ ++ ros::NodeHandle _nh; ++ UpdatePlanningParams(_nh); ++ ++ tf::StampedTransform transform; ++ PlannerHNS::ROSHelpers::GetTransformFromTF("map", "world", transform); ++ m_OriginPos.position.x = transform.getOrigin().x(); ++ m_OriginPos.position.y = transform.getOrigin().y(); ++ m_OriginPos.position.z = transform.getOrigin().z(); ++ ++ pub_LocalTrajectories = nh.advertise("local_trajectories", 1); ++ pub_LocalTrajectoriesWithPoseTwist = nh.advertise("local_trajectories_with_pose_twist", 1); ++ pub_LocalTrajectoriesRviz = nh.advertise("local_trajectories_gen_rviz", 1); ++ ++ sub_initialpose = nh.subscribe("/initialpose", 1, &TrajectoryGen::callbackGetInitPose, this); ++ // sub_current_pose = nh.subscribe("/current_pose", 10, &TrajectoryGen::callbackGetCurrentPose, this); ++ ++ int bVelSource = 1; ++ _nh.getParam("/op_trajectory_generator/velocitySource", bVelSource); ++ if(bVelSource == 0) ++ sub_robot_odom = nh.subscribe("/odom", 10, &TrajectoryGen::callbackGetRobotOdom, this); ++ // else if(bVelSource == 1) ++ // sub_current_velocity = nh.subscribe("/current_velocity", 10, &TrajectoryGen::callbackGetVehicleStatus, this); ++ else if(bVelSource == 2) ++ sub_can_info = nh.subscribe("/can_info", 10, &TrajectoryGen::callbackGetCANInfo, this); ++ ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); ++ ++ sub_pose_twist = nh.subscribe("/rubis_current_pose_twist", 10, &TrajectoryGen::callbackGetCurrentPoseTwist, this); ++} ++ ++TrajectoryGen::~TrajectoryGen() ++{ ++} ++ ++void TrajectoryGen::UpdatePlanningParams(ros::NodeHandle& _nh) ++{ ++ _nh.getParam("/op_trajectory_generator/samplingTipMargin", m_PlanningParams.carTipMargin); ++ _nh.getParam("/op_trajectory_generator/samplingOutMargin", m_PlanningParams.rollInMargin); ++ _nh.getParam("/op_trajectory_generator/samplingSpeedFactor", m_PlanningParams.rollInSpeedFactor); ++ _nh.getParam("/op_trajectory_generator/enableHeadingSmoothing", m_PlanningParams.enableHeadingSmoothing); ++ ++ _nh.getParam("/op_common_params/enableSwerving", m_PlanningParams.enableSwerving); ++ if(m_PlanningParams.enableSwerving) ++ m_PlanningParams.enableFollowing = true; ++ else ++ _nh.getParam("/op_common_params/enableFollowing", m_PlanningParams.enableFollowing); ++ ++ _nh.getParam("/op_common_params/enableTrafficLightBehavior", m_PlanningParams.enableTrafficLightBehavior); ++ _nh.getParam("/op_common_params/enableStopSignBehavior", m_PlanningParams.enableStopSignBehavior); ++ ++ _nh.getParam("/op_common_params/maxVelocity", m_PlanningParams.maxSpeed); ++ _nh.getParam("/op_common_params/minVelocity", m_PlanningParams.minSpeed); ++ _nh.getParam("/op_common_params/maxLocalPlanDistance", m_PlanningParams.microPlanDistance); ++ ++ _nh.getParam("/op_common_params/pathDensity", m_PlanningParams.pathDensity); ++ _nh.getParam("/op_common_params/rollOutDensity", m_PlanningParams.rollOutDensity); ++ if(m_PlanningParams.enableSwerving) ++ _nh.getParam("/op_common_params/rollOutsNumber", m_PlanningParams.rollOutNumber); ++ else ++ m_PlanningParams.rollOutNumber = 0; ++ ++ _nh.getParam("/op_common_params/horizonDistance", m_PlanningParams.horizonDistance); ++ _nh.getParam("/op_common_params/minFollowingDistance", m_PlanningParams.minFollowingDistance); ++ _nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid); ++ _nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid); ++ _nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor); ++ ++ _nh.getParam("/op_common_params/smoothingDataWeight", m_PlanningParams.smoothingDataWeight); ++ _nh.getParam("/op_common_params/smoothingSmoothWeight", m_PlanningParams.smoothingSmoothWeight); ++ ++ _nh.getParam("/op_common_params/horizontalSafetyDistance", m_PlanningParams.horizontalSafetyDistancel); ++ _nh.getParam("/op_common_params/verticalSafetyDistance", m_PlanningParams.verticalSafetyDistance); ++ ++ _nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange); ++ ++ _nh.getParam("/op_common_params/width", m_CarInfo.width); ++ _nh.getParam("/op_common_params/length", m_CarInfo.length); ++ _nh.getParam("/op_common_params/wheelBaseLength", m_CarInfo.wheel_base); ++ _nh.getParam("/op_common_params/turningRadius", m_CarInfo.turning_radius); ++ _nh.getParam("/op_common_params/maxSteerAngle", m_CarInfo.max_steer_angle); ++ _nh.getParam("/op_common_params/maxAcceleration", m_CarInfo.max_acceleration); ++ _nh.getParam("/op_common_params/maxDeceleration", m_CarInfo.max_deceleration); ++ ++ m_CarInfo.max_speed_forward = m_PlanningParams.maxSpeed; ++ m_CarInfo.min_speed_forward = m_PlanningParams.minSpeed; ++ ++} ++ ++void TrajectoryGen::callbackGetInitPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) ++{ ++ if(!bInitPos) ++ { ++ m_InitPos = PlannerHNS::WayPoint(msg->pose.pose.position.x+m_OriginPos.position.x, ++ msg->pose.pose.position.y+m_OriginPos.position.y, ++ msg->pose.pose.position.z+m_OriginPos.position.z, ++ tf::getYaw(msg->pose.pose.orientation)); ++ m_CurrentPos = m_InitPos; ++ bInitPos = true; ++ } ++} ++ ++// void TrajectoryGen::callbackGetCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) ++// { ++// m_CurrentPos = PlannerHNS::WayPoint(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, tf::getYaw(msg->pose.orientation)); ++// m_InitPos = m_CurrentPos; ++// bNewCurrentPos = true; ++// bInitPos = true; ++// } ++ ++// void TrajectoryGen::callbackGetVehicleStatus(const geometry_msgs::TwistStampedConstPtr& msg) ++// { ++// m_VehicleStatus.speed = msg->twist.linear.x; ++// m_CurrentPos.v = m_VehicleStatus.speed; ++// if(fabs(msg->twist.linear.x) > 0.25) ++// m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.angular.z/msg->twist.linear.x); ++// UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++// bVehicleStatus = true; ++// } ++ ++void TrajectoryGen::callbackGetCurrentPoseTwist(const rubis_msgs::PoseTwistStampedPtr& msg){ ++ // Before spinOnce ++ rubis::start_task_profiling(); ++ ++ // Callback ++ m_CurrentPos = PlannerHNS::WayPoint(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z, tf::getYaw(msg->pose.pose.orientation)); ++ m_InitPos = m_CurrentPos; ++ bNewCurrentPos = true; ++ bInitPos = true; ++ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_CurrentPos.v = m_VehicleStatus.speed; ++ if(fabs(msg->twist.twist.linear.x) > 0.25) ++ m_VehicleStatus.steer = atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++ ++ // After spinOnce ++ if(bInitPos && m_GlobalPaths.size()>0) ++ { ++ m_GlobalPathSections.clear(); ++ ++ for(unsigned int i = 0; i < m_GlobalPaths.size(); i++) ++ { ++ t_centerTrajectorySmoothed.clear(); ++ PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_GlobalPaths.at(i), m_CurrentPos, m_PlanningParams.horizonDistance , ++ m_PlanningParams.pathDensity ,t_centerTrajectorySmoothed); ++ ++ m_GlobalPathSections.push_back(t_centerTrajectorySmoothed); ++ } ++ ++ std::vector sampledPoints_debug; ++ m_Planner.GenerateRunoffTrajectory(m_GlobalPathSections, m_CurrentPos, ++ m_PlanningParams.enableLaneChange, ++ m_VehicleStatus.speed, ++ m_PlanningParams.microPlanDistance, ++ m_PlanningParams.maxSpeed, ++ m_PlanningParams.minSpeed, ++ m_PlanningParams.carTipMargin, ++ m_PlanningParams.rollInMargin, ++ m_PlanningParams.rollInSpeedFactor, ++ m_PlanningParams.pathDensity, ++ m_PlanningParams.rollOutDensity, ++ m_PlanningParams.rollOutNumber, ++ m_PlanningParams.smoothingDataWeight, ++ m_PlanningParams.smoothingSmoothWeight, ++ m_PlanningParams.smoothingToleranceError, ++ m_PlanningParams.speedProfileFactor, ++ m_PlanningParams.enableHeadingSmoothing, ++ -1 , -1, ++ m_RollOuts, sampledPoints_debug); ++ ++ rubis_msgs::LaneArrayWithPoseTwist local_lanes; ++ for(unsigned int i=0; i < m_RollOuts.size(); i++) ++ { ++ for(unsigned int j=0; j < m_RollOuts.at(i).size(); j++) ++ { ++ autoware_msgs::Lane lane; ++ PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(m_RollOuts.at(i).at(j), m_CurrentPos, m_PlanningParams.minSpeed, m_PlanningParams.microPlanDistance); ++ PlannerHNS::ROSHelpers::ConvertFromLocalLaneToAutowareLane(m_RollOuts.at(i).at(j), lane); ++ lane.closest_object_distance = 0; ++ lane.closest_object_velocity = 0; ++ lane.cost = 0; ++ lane.is_blocked = false; ++ lane.lane_index = i; ++ local_lanes.lane_array.lanes.push_back(lane); ++ } ++ } ++ ++ rubis::instance_ = msg->instance; ++ local_lanes.instance = rubis::instance_; ++ local_lanes.pose = msg->pose; ++ local_lanes.twist = msg->twist; ++ ++ pub_LocalTrajectoriesWithPoseTwist.publish(local_lanes); ++ pub_LocalTrajectories.publish(local_lanes.lane_array); ++ ++ } ++ else{ ++ sub_GlobalPlannerPaths = nh.subscribe("/lane_waypoints_array", 1, &TrajectoryGen::callbackGetGlobalPlannerPath, this); ++ ++ visualization_msgs::MarkerArray all_rollOuts; ++ PlannerHNS::ROSHelpers::TrajectoriesToMarkers(m_RollOuts, all_rollOuts); ++ pub_LocalTrajectoriesRviz.publish(all_rollOuts); ++ ++ rubis::stop_task_profiling(0, 0); ++ } ++} ++ ++ ++void TrajectoryGen::callbackGetCANInfo(const autoware_can_msgs::CANInfoConstPtr &msg) ++{ ++ m_VehicleStatus.speed = msg->speed/3.6; ++ m_VehicleStatus.steer = msg->angle * m_CarInfo.max_steer_angle / m_CarInfo.max_steer_value; ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void TrajectoryGen::callbackGetRobotOdom(const nav_msgs::OdometryConstPtr& msg) ++{ ++ m_VehicleStatus.speed = msg->twist.twist.linear.x; ++ m_VehicleStatus.steer += atan(m_CarInfo.wheel_base * msg->twist.twist.angular.z/msg->twist.twist.linear.x); ++ UtilityHNS::UtilityH::GetTickCount(m_VehicleStatus.tStamp); ++ bVehicleStatus = true; ++} ++ ++void TrajectoryGen::callbackGetGlobalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg) ++{ ++ if(msg->lanes.size() > 0) ++ { ++ bool bOldGlobalPath = m_GlobalPaths.size() == msg->lanes.size(); ++ ++ m_GlobalPaths.clear(); ++ ++ for(unsigned int i = 0 ; i < msg->lanes.size(); i++) ++ { ++ PlannerHNS::ROSHelpers::ConvertFromAutowareLaneToLocalLane(msg->lanes.at(i), m_temp_path); ++ ++ PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_temp_path); ++ m_GlobalPaths.push_back(m_temp_path); ++ ++ if(bOldGlobalPath) ++ { ++ bOldGlobalPath = PlannerHNS::PlanningHelpers::CompareTrajectories(m_temp_path, m_GlobalPaths.at(i)); ++ } ++ } ++ ++ if(!bOldGlobalPath) ++ { ++ bWayGlobalPath = true; ++ std::cout << "Received New Global Path Generator ! " << std::endl; ++ } ++ else ++ { ++ m_GlobalPaths.clear(); ++ } ++ } ++} ++ ++void TrajectoryGen::MainLoop() ++{ ++ ros::NodeHandle private_nh("~"); ++ ++ // Scheduling Setup ++ std::string task_response_time_filename; ++ int rate; ++ double task_minimum_inter_release_time; ++ double task_execution_time; ++ double task_relative_deadline; ++ ++ private_nh.param("/op_trajectory_generator/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/op_trajectory_generator.csv"); ++ private_nh.param("/op_trajectory_generator/rate", rate, 10); ++ private_nh.param("/op_trajectory_generator/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); ++ private_nh.param("/op_trajectory_generator/task_execution_time", task_execution_time, (double)10); ++ private_nh.param("/op_trajectory_generator/task_relative_deadline", task_relative_deadline, (double)10); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ PlannerHNS::WayPoint prevState, state_change; ++ ++ ros::spin(); ++} ++ ++} +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt +index 97cc58e4..deef12e6 100644 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/CMakeLists.txt +@@ -60,20 +60,4 @@ install( + install( + DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +-) +- +-if(CATKIN_ENABLE_TESTING) +- find_package(rostest REQUIRED) +- +- add_rostest_gtest( +- test-pure_pursuit +- test/test_pure_pursuit.test +- test/src/test_pure_pursuit.cpp +- src/pure_pursuit_core.cpp +- src/pure_pursuit.cpp src/pure_pursuit_viz.cpp +- ) +- add_dependencies(test-pure_pursuit ${catkin_EXPORTED_TARGETS}) +- target_link_libraries(test-pure_pursuit ${catkin_LIBRARIES}) +- +- roslint_add_test() +-endif() ++) +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h +index bab5c953..6a56fee1 100644 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit.h +@@ -24,6 +24,7 @@ + #include + #include + #include ++#include "autoware_msgs/Lane.h" + + // C++ includes + #include +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h +index 4e2511ef..3e6700e2 100755 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/include/pure_pursuit/pure_pursuit_core.h +@@ -27,6 +27,7 @@ + #include + #include + #include ++#include + + #include + #include +@@ -44,6 +45,8 @@ + + #include + ++#include "rubis_msgs/LaneWithPoseTwist.h" ++ + #ifdef IONIC + #include + #endif +@@ -97,7 +100,7 @@ private: + pub11_, pub12_, pub13_, pub14_, pub15_, pub16_, pub17_, pub18_; + + // subscriber +- ros::Subscriber sub1_, pose_sub_, rubis_pose_sub_, sub3_, velocity_sub_, car_ctrl_output_sub; ++ ros::Subscriber sub1_, sub3_, pose_twist_sub_, final_waypoints_with_pose_twist_sub, car_ctrl_output_sub; + + // constant + const int LOOP_RATE_; // processing frequency +@@ -125,17 +128,14 @@ private: + // Added by PHY + bool dynamic_param_flag_; + std::vector dynamic_params; ++ autoware_msgs::Lane lane_; + + // callbacks +- void callbackFromConfig( +- const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config); +- void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); +- void callbackFromRubisCurrentPose(const rubis_msgs::PoseStampedConstPtr& _msg); ++ void callbackFromConfig(const autoware_config_msgs::ConfigWaypointFollowerConstPtr& config); + void callbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg); +- +- #ifdef SVL +- void callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg); +- #endif ++ void CallbackTwistPose(const rubis_msgs::PoseTwistStampedConstPtr& msg); ++ void CallbackFinalWaypointsWithPoseTwist(const rubis_msgs::LaneWithPoseTwistConstPtr& msg); ++ void _CallbackFinalWaypointsWithPoseTwist(); + + #ifdef IONIC + void callbackCtrlOutput(const can_data_msgs::Car_ctrl_output::ConstPtr &msg); +@@ -160,7 +160,7 @@ private: + const std::vector& waypoints) const; + void connectVirtualLastWaypoints( + autoware_msgs::Lane* expand_lane, LaneDirection direction); +- inline void updateCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg); ++ inline void updateCurrentPose(geometry_msgs::PoseStampedConstPtr& msg); + + // Added by PHY + void setLookaheadParamsByVel(); +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch +index 359db355..0e4c9427 100644 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit.launch +@@ -7,7 +7,6 @@ + + + +- + + + +@@ -15,7 +14,6 @@ + + + +- + + + +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch +index 3ac38797..e04e165e 100644 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/launch/pure_pursuit_params.launch +@@ -7,7 +7,6 @@ + + + +- + + + +@@ -15,7 +14,6 @@ + + + +- + + + +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp +old mode 100755 +new mode 100644 +index 0532fe93..1e105f4a +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp ++++ b/autoware.ai/src/autoware/core_planning/pure_pursuit/src/pure_pursuit_core.cpp +@@ -142,7 +142,6 @@ void PurePursuitNode::initForROS() + nh_.param("vehicle_info/wheel_base", wheel_base_, 2.7); + + private_nh_.param("/pure_pursuit/dynamic_params_flag", dynamic_param_flag_, false); +- private_nh_.param("/pure_pursuit/instance_mode", rubis::instance_mode_, 0); + + if(dynamic_param_flag_){ + XmlRpc::XmlRpcValue xml_list; +@@ -167,26 +166,15 @@ void PurePursuitNode::initForROS() + + + // setup subscriber +- sub1_ = nh_.subscribe("final_waypoints", 10, +- &PurePursuitNode::callbackFromWayPoints, this); +- +- if(rubis::instance_mode_) rubis_pose_sub_ = nh_.subscribe("rubis_current_pose", 10, &PurePursuitNode::callbackFromRubisCurrentPose, this); +- else pose_sub_ = nh_.subscribe("current_pose", 10, &PurePursuitNode::callbackFromCurrentPose, this); ++ pose_twist_sub_ = nh_.subscribe("/rubis_current_pose_twist", 1, &PurePursuitNode::CallbackTwistPose, this); ++ final_waypoints_with_pose_twist_sub = nh_.subscribe("/final_waypoints_with_pose_twist", 1, &PurePursuitNode::CallbackFinalWaypointsWithPoseTwist, this); // Def: 10 + + sub3_ = nh_.subscribe("config/waypoint_follower", 10, + &PurePursuitNode::callbackFromConfig, this); + +- #ifdef SVL +- velocity_sub_ = nh_.subscribe("current_velocity", 10, &PurePursuitNode::callbackFromCurrentVelocity, this); +- #endif +- +- #ifdef IONIC +- car_ctrl_output_sub = nh_.subscribe("car_ctrl_output", 10, &PurePursuitNode::callbackCtrlOutput, this); +- #endif +- + // setup publisher + twist_pub_ = nh_.advertise("twist_raw", 10); +- if(rubis::instance_mode_) rubis_twist_pub_ = nh_.advertise("rubis_twist_raw", 10); ++ rubis_twist_pub_ = nh_.advertise("rubis_twist_raw", 10); + + pub2_ = nh_.advertise("ctrl_raw", 10); + pub11_ = nh_.advertise("next_waypoint_mark", 0); +@@ -208,99 +196,32 @@ void PurePursuitNode::run() + { + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); + std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- +- private_nh.param("/pure_pursuit/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/pure_pursuit/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/pure_pursuit/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pure_pursuit.csv"); +- private_nh.param("/pure_pursuit/rate", rate, 10); +- private_nh.param("/pure_pursuit/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/pure_pursuit/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/pure_pursuit/task_relative_deadline", task_relative_deadline, (double)10); +- +- ROS_INFO_STREAM("pure pursuit start"); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- ros::Rate loop_rate(LOOP_RATE_); +- if(!task_scheduling_flag && !task_profiling_flag) loop_rate = ros::Rate(rate); +- +- while (ros::ok()) +- { +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- if (!is_pose_set_ || !is_waypoint_set_ || !is_velocity_set_) +- { +- // ROS_WARN("Necessary topics are not subscribed yet ... "); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pure_pursuit.csv"); + +- loop_rate.sleep(); +- continue; +- } +- +- pp_.setLookaheadDistance(computeLookaheadDistance()); +- pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_); +- +- double kappa = 0; +- bool can_get_curvature = pp_.canGetCurvature(&kappa); +- +- publishTwistStamped(can_get_curvature, kappa); +- publishControlCommandStamped(can_get_curvature, kappa); +- // for visualization with Rviz +- pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint())); +- pub13_.publish(displaySearchRadius( +- pp_.getCurrentPose().position, pp_.getLookaheadDistance())); +- pub12_.publish(displayNextTarget(pp_.getPoseOfNextTarget())); +- pub15_.publish(displayTrajectoryCircle( +- waypoint_follower::generateTrajectoryCircle( +- pp_.getPoseOfNextTarget(), pp_.getCurrentPose()))); +- if (add_virtual_end_waypoints_) +- { +- pub18_.publish( +- displayExpandWaypoints(pp_.getCurrentWaypoints(), expand_size_)); +- } +- std_msgs::Float32 angular_gravity_msg; +- angular_gravity_msg.data = +- computeAngularGravity(computeCommandVelocity(), kappa); +- pub16_.publish(angular_gravity_msg); +- +- publishDeviationCurrentPosition( +- pp_.getCurrentPose().position, pp_.getCurrentWaypoints()); +- +- is_pose_set_ = false; +- is_velocity_set_ = false; +- is_waypoint_set_ = false; +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- +- if(rubis::sched::is_task_ready_ == TASK_READY && rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- loop_rate.sleep(); +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + } + ++ + void PurePursuitNode::publishTwistStamped( + const bool& can_get_curvature, const double& kappa) const + { +@@ -310,15 +231,11 @@ void PurePursuitNode::publishTwistStamped( + ts.twist.angular.z = can_get_curvature ? kappa * ts.twist.linear.x : 0; + twist_pub_.publish(ts); + +- if(rubis::instance_mode_ && rubis::instance_mode_ != RUBIS_NO_INSTANCE){ +- rubis_msgs::TwistStamped rubis_ts; +- rubis_ts.instance = rubis::instance_; +- rubis_ts.msg = ts; +- rubis_twist_pub_.publish(rubis_ts); +- } +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ rubis_msgs::TwistStamped rubis_ts; ++ rubis_ts.instance = rubis::instance_; ++ rubis_ts.obj_instance = rubis::obj_instance_; ++ rubis_ts.msg = ts; ++ rubis_twist_pub_.publish(rubis_ts); + } + + void PurePursuitNode::publishControlCommandStamped( +@@ -395,6 +312,7 @@ double PurePursuitNode::computeCommandVelocity() const + + double PurePursuitNode::computeCommandAccel() const + { ++ if(pp_.getCurrentWaypoints().size() < 2) return 0.0; + const geometry_msgs::Pose current_pose = pp_.getCurrentPose(); + const geometry_msgs::Pose target_pose = + pp_.getCurrentWaypoints().at(1).pose.pose; +@@ -406,6 +324,7 @@ double PurePursuitNode::computeCommandAccel() const + const double v0 = current_linear_velocity_; + const double v = computeCommandVelocity(); + const double a = getSgn() * (v * v - v0 * v0) / (2 * x); ++ + return a; + } + +@@ -449,7 +368,7 @@ void PurePursuitNode::publishDeviationCurrentPosition( + pub17_.publish(msg); + } + +-inline void PurePursuitNode::updateCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg){ ++inline void PurePursuitNode::updateCurrentPose(geometry_msgs::PoseStampedConstPtr& msg){ + #ifndef USE_WAYPOINT_ORIENTATION + pp_.setCurrentPose(msg); + #else +@@ -462,27 +381,6 @@ inline void PurePursuitNode::updateCurrentPose(const geometry_msgs::PoseStampedC + is_pose_set_ = true; + } + +-void PurePursuitNode::callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr& msg) +-{ +- updateCurrentPose(msg); +-} +- +-void PurePursuitNode::callbackFromRubisCurrentPose(const rubis_msgs::PoseStampedConstPtr& _msg) +-{ +- geometry_msgs::PoseStampedConstPtr msg = boost::make_shared(_msg->msg); +- rubis::instance_ = _msg->instance; +- updateCurrentPose(msg); +-} +- +-#ifdef SVL +-void PurePursuitNode::callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr& msg) +-{ +- current_linear_velocity_ = msg->twist.linear.x; +- pp_.setCurrentVelocity(current_linear_velocity_); +- is_velocity_set_ = true; +-} +-#endif +- + #ifdef IONIC + void PurePursuitNode::callbackCtrlOutput(const can_data_msgs::Car_ctrl_output::ConstPtr &msg) + { +@@ -526,19 +424,32 @@ double PurePursuitNode::findWayPointVelocity(autoware_msgs::Waypoint msg){ + return way_points_velocity_[idx]; + } + +-void PurePursuitNode::callbackFromWayPoints( +- const autoware_msgs::LaneConstPtr& msg) ++void PurePursuitNode::CallbackTwistPose(const rubis_msgs::PoseTwistStampedConstPtr& msg) + { ++ rubis::start_task_profiling(); ++ rubis::instance_ = msg->instance; ++ ++ // Update pose ++ geometry_msgs::PoseStampedConstPtr pose_ptr(new geometry_msgs::PoseStamped(msg->pose)); ++ updateCurrentPose(pose_ptr); ++ ++ // Update twist ++ current_linear_velocity_ = msg->twist.twist.linear.x; ++ pp_.setCurrentVelocity(current_linear_velocity_); ++ is_velocity_set_ = true; ++ ++ if(lane_.waypoints.size() < 1) return; ++ + if(use_algorithm_){ +- command_linear_velocity_ = findWayPointVelocity(msg->waypoints.at(0)); ++ command_linear_velocity_ = findWayPointVelocity(lane_.waypoints.at(0)); + } + else{ +- command_linear_velocity_ = (!msg->waypoints.empty()) ? msg->waypoints.at(0).twist.twist.linear.x : 0; ++ command_linear_velocity_ = (!lane_.waypoints.empty()) ? lane_.waypoints.at(0).twist.twist.linear.x : 0; + } + +- geometry_msgs::Point curr_point = msg->waypoints.at(0).pose.pose.position; +- geometry_msgs::Point near_point = msg->waypoints.at(std::min(3, (int)msg->waypoints.size() - 1)).pose.pose.position; +- geometry_msgs::Point far_point = msg->waypoints.at(std::min(30, (int)msg->waypoints.size() - 1)).pose.pose.position; ++ geometry_msgs::Point curr_point = lane_.waypoints.at(0).pose.pose.position; ++ geometry_msgs::Point near_point = lane_.waypoints.at(std::min(3, (int)lane_.waypoints.size() - 1)).pose.pose.position; ++ geometry_msgs::Point far_point = lane_.waypoints.at(std::min(30, (int)lane_.waypoints.size() - 1)).pose.pose.position; + + double deg_1 = atan2((near_point.y - curr_point.y), (near_point.x - curr_point.x)) / 3.14 * 180; + double deg_2 = atan2((far_point.y - curr_point.y), (far_point.x - curr_point.x)) / 3.14 * 180; +@@ -549,15 +460,58 @@ void PurePursuitNode::callbackFromWayPoints( + + angle_diff_ = angle_diff; + ++ // Update waypoints ++ _CallbackFinalWaypointsWithPoseTwist(); ++ ++ // After spinOnce ++ pp_.setLookaheadDistance(computeLookaheadDistance()); ++ pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_); ++ ++ double kappa = 0; ++ bool can_get_curvature = pp_.canGetCurvature(&kappa); ++ ++ publishTwistStamped(can_get_curvature, kappa); ++ publishControlCommandStamped(can_get_curvature, kappa); ++ // for visualization with Rviz ++ pub11_.publish(displayNextWaypoint(pp_.getPoseOfNextWaypoint())); ++ pub13_.publish(displaySearchRadius( ++ pp_.getCurrentPose().position, pp_.getLookaheadDistance())); ++ pub12_.publish(displayNextTarget(pp_.getPoseOfNextTarget())); ++ pub15_.publish(displayTrajectoryCircle( ++ waypoint_follower::generateTrajectoryCircle( ++ pp_.getPoseOfNextTarget(), pp_.getCurrentPose()))); ++ if (add_virtual_end_waypoints_) ++ { ++ pub18_.publish( ++ displayExpandWaypoints(pp_.getCurrentWaypoints(), expand_size_)); ++ } ++ std_msgs::Float32 angular_gravity_msg; ++ angular_gravity_msg.data = ++ computeAngularGravity(computeCommandVelocity(), kappa); ++ pub16_.publish(angular_gravity_msg); ++ ++ publishDeviationCurrentPosition( ++ pp_.getCurrentPose().position, pp_.getCurrentWaypoints()); ++ ++ is_pose_set_ = false; ++ is_velocity_set_ = false; ++ is_waypoint_set_ = false; ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ ++} ++ ++void PurePursuitNode::_CallbackFinalWaypointsWithPoseTwist() ++{ + if(dynamic_param_flag_){ + setLookaheadParamsByVel(); + } + + if (add_virtual_end_waypoints_) + { +- const LaneDirection solved_dir = getLaneDirection(*msg); ++ const LaneDirection solved_dir = getLaneDirection(lane_); + direction_ = (solved_dir != LaneDirection::Error) ? solved_dir : direction_; +- autoware_msgs::Lane expanded_lane(*msg); ++ autoware_msgs::Lane expanded_lane(lane_); + expand_size_ = -expanded_lane.waypoints.size(); + connectVirtualLastWaypoints(&expanded_lane, direction_); + expand_size_ += expanded_lane.waypoints.size(); +@@ -566,15 +520,21 @@ void PurePursuitNode::callbackFromWayPoints( + } + else + { +- pp_.setCurrentWaypoints(msg->waypoints); ++ pp_.setCurrentWaypoints(lane_.waypoints); + } + is_waypoint_set_ = true; + + #ifdef USE_WAYPOINT_ORIENTATION +- waypoint_pose_ = msg->waypoints[0].pose; ++ waypoint_pose_ = lane_.waypoints[0].pose; + #endif + } + ++void PurePursuitNode::CallbackFinalWaypointsWithPoseTwist(const rubis_msgs::LaneWithPoseTwistConstPtr& msg) ++{ ++ rubis::obj_instance_ = msg->obj_instance; ++ lane_ = msg->lane; ++} ++ + void PurePursuitNode::connectVirtualLastWaypoints( + autoware_msgs::Lane* lane, LaneDirection direction) + { +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp b/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp +deleted file mode 100644 +index 5df8b48e..00000000 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/src/test_pure_pursuit.cpp ++++ /dev/null +@@ -1,121 +0,0 @@ +-/* +- * Copyright 2018-2019 Autoware Foundation. All rights reserved. +- * +- * Licensed under the Apache License, Version 2.0 (the "License"); +- * you may not use this file except in compliance with the License. +- * You may obtain a copy of the License at +- * +- * http://www.apache.org/licenses/LICENSE-2.0 +- * +- * Unless required by applicable law or agreed to in writing, software +- * distributed under the License is distributed on an "AS IS" BASIS, +- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +- * See the License for the specific language governing permissions and +- * limitations under the License. +- */ +- +-#include +-#include +-#include +- +-namespace waypoint_follower +-{ +-class PurePursuitNodeTestSuite : public ::testing::Test +-{ +-protected: +- std::unique_ptr obj_; +- virtual void SetUp() +- { +- obj_ = std::unique_ptr(new PurePursuitNode()); +- obj_->add_virtual_end_waypoints_ = true; +- } +- virtual void TearDown() +- { +- obj_.reset(); +- } +- +-public: +- PurePursuitNodeTestSuite() {} +- ~PurePursuitNodeTestSuite() {} +- LaneDirection getDirection() +- { +- return obj_->direction_; +- } +- void ppCallbackFromWayPoints(const autoware_msgs::LaneConstPtr& msg) +- { +- obj_->callbackFromWayPoints(msg); +- } +- void ppConnectVirtualLastWaypoints( +- autoware_msgs::Lane* expand_lane, LaneDirection direction) +- { +- obj_->connectVirtualLastWaypoints(expand_lane, direction); +- } +-}; +- +-TEST_F(PurePursuitNodeTestSuite, inputPositivePath) +-{ +- autoware_msgs::Lane original_lane; +- original_lane.waypoints.resize(3, autoware_msgs::Waypoint()); +- for (int i = 0; i < 3; i++) +- { +- original_lane.waypoints[i].pose.pose.position.x = i; +- original_lane.waypoints[i].pose.pose.orientation = +- tf::createQuaternionMsgFromYaw(0.0); +- } +- const autoware_msgs::LaneConstPtr +- lp(boost::make_shared(original_lane)); +- ppCallbackFromWayPoints(lp); +- ASSERT_EQ(getDirection(), LaneDirection::Forward) +- << "direction is not matching to positive lane."; +-} +- +-TEST_F(PurePursuitNodeTestSuite, inputNegativePath) +-{ +- autoware_msgs::Lane original_lane; +- original_lane.waypoints.resize(3, autoware_msgs::Waypoint()); +- for (int i = 0; i < 3; i++) +- { +- original_lane.waypoints[i].pose.pose.position.x = -i; +- original_lane.waypoints[i].pose.pose.orientation = +- tf::createQuaternionMsgFromYaw(0.0); +- } +- const autoware_msgs::LaneConstPtr +- lp(boost::make_shared(original_lane)); +- ppCallbackFromWayPoints(lp); +- ASSERT_EQ(getDirection(), LaneDirection::Backward) +- << "direction is not matching to negative lane."; +-} +-// If original lane is empty, new lane is also empty. +-TEST_F(PurePursuitNodeTestSuite, inputEmptyLane) +-{ +- autoware_msgs::Lane original_lane, new_lane; +- ppConnectVirtualLastWaypoints(&new_lane, LaneDirection::Forward); +- ASSERT_EQ(original_lane.waypoints.size(), new_lane.waypoints.size()) +- << "Input empty lane, and output is not empty"; +-} +- +-// If the original lane exceeds 2 points, +-// the additional part will be updated at +-// the interval of the first 2 points. +-TEST_F(PurePursuitNodeTestSuite, inputNormalLane) +-{ +- autoware_msgs::Lane original_lane; +- original_lane.waypoints.resize(2, autoware_msgs::Waypoint()); +- for (int i = 0; i < 2; i++) +- { +- original_lane.waypoints[i].pose.pose.position.x = i; +- } +- autoware_msgs::Lane new_lane(original_lane); +- ppConnectVirtualLastWaypoints(&new_lane, LaneDirection::Forward); +- +- ASSERT_LT(original_lane.waypoints.size(), new_lane.waypoints.size()) +- << "Fail to expand waypoints"; +-} +-} // namespace waypoint_follower +- +-int main(int argc, char** argv) +-{ +- testing::InitGoogleTest(&argc, argv); +- ros::init(argc, argv, "PurePursuitTest"); +- return RUN_ALL_TESTS(); +-} +diff --git a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test b/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test +deleted file mode 100644 +index caba2948..00000000 +--- a/autoware.ai/src/autoware/core_planning/pure_pursuit/test/test_pure_pursuit.test ++++ /dev/null +@@ -1,3 +0,0 @@ +- +- +- +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h b/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h +index 997b984c..6c05ce54 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/include/twist_filter/twist_filter_node.h +@@ -50,8 +50,9 @@ private: + ros::Subscriber twist_sub_, rubis_twist_sub_, ctrl_sub_, config_sub_; + // Added by PHY + ros::Subscriber emergency_stop_sub_; ++ autoware_msgs::ControlCommandStampedConstPtr ctrl_cmd_ptr_; + +- bool emergency_stop_; ++ bool emergency_stop_, current_emergency_stop_; + int max_stop_count_; + int current_stop_count_; + +@@ -60,13 +61,16 @@ private: + void rubisTwistCmdCallback(const rubis_msgs::TwistStampedConstPtr& _msg); + inline void publishTwist(const geometry_msgs::TwistStampedConstPtr& msg); + void ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg); ++ void _ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg); + void checkTwist(const twist_filter::Twist twist, const twist_filter::Twist twist_prev, const double& dt); + void checkCtrl(const twist_filter::Ctrl ctrl, const twist_filter::Ctrl ctrl_prev, const double& dt); + + //Added by PHY + void emergencyStopCallback(const std_msgs::Bool& msg); ++ void _emergencyStopCallback(); + }; + + } // namespace twist_filter_node + + #endif // TWIST_FILTER_TWIST_FILTER_NODE_H ++ +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch +index 81dc1fed..b3b8806e 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter.launch +@@ -8,7 +8,6 @@ + + + +- + + + +@@ -24,12 +23,10 @@ + + + +- + + +- ++ + + +- + + +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch +index 0b470a12..b3b8806e 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/launch/twist_filter_params.launch +@@ -8,7 +8,6 @@ + + + +- + + + +@@ -24,12 +23,10 @@ + + + +- + + +- ++ + + +- + + +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp +index 89165fd1..c447e46f 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node.cpp +@@ -29,27 +29,19 @@ TwistFilterNode::TwistFilterNode() : nh_(), private_nh_("~") + nh_.param("twist_filter/lowpass_gain_angular_z", twist_filter_config.lowpass_gain_angular_z, 0.0); + nh_.param("twist_filter/lowpass_gain_steering_angle", twist_filter_config.lowpass_gain_steering_angle, 0.0); + nh_.param("twist_filter/max_stop_count", max_stop_count_, 30); +- nh_.param("twist_filter/instance_mode", rubis::instance_mode_, 0); + twist_filter_ptr_ = std::make_shared(twist_filter_config); + emergency_stop_ = false; + current_stop_count_ = 0; + + // Subscribe +- if(rubis::instance_mode_) rubis_twist_sub_ = nh_.subscribe("rubis_twist_raw", 1, &TwistFilterNode::rubisTwistCmdCallback, this); +- else twist_sub_ = nh_.subscribe("twist_raw", 1, &TwistFilterNode::twistCmdCallback, this); ++ rubis_twist_sub_ = nh_.subscribe("rubis_twist_raw", 1, &TwistFilterNode::rubisTwistCmdCallback, this); + ctrl_sub_ = nh_.subscribe("ctrl_raw", 1, &TwistFilterNode::ctrlCmdCallback, this); + config_sub_ = nh_.subscribe("config/twist_filter", 10, &TwistFilterNode::configCallback, this); + emergency_stop_sub_ = nh_.subscribe("emergency_stop", 1 ,&TwistFilterNode::emergencyStopCallback, this); + +- /* RT Scheduling setup */ +- // twist_sub_ = nh_.subscribe("twist_raw", 1, &TwistFilterNode::twistCmdCallback, this); +- // ctrl_sub_ = nh_.subscribe("ctrl_raw", 1, &TwistFilterNode::ctrlCmdCallback, this); +- // config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistFilterNode::configCallback, this); //origin 10 +- // emergency_stop_sub_ = nh_.subscribe("emergency_stop", 1 ,&TwistFilterNode::emergencyStopCallback, this); +- + // Publish + twist_pub_ = nh_.advertise("twist_cmd", 5); +- if(rubis::instance_mode_) rubis_twist_pub_ = nh_.advertise("rubis_twist_cmd", 5); ++ rubis_twist_pub_ = nh_.advertise("rubis_twist_cmd", 5); + ctrl_pub_ = nh_.advertise("ctrl_cmd", 5); + twist_lacc_limit_debug_pub_ = private_nh_.advertise("limitation_debug/twist/lateral_accel", 5); + twist_ljerk_limit_debug_pub_ = private_nh_.advertise("limitation_debug/twist/lateral_jerk", 5); +@@ -113,6 +105,8 @@ inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConst + // Smoothed value publish + geometry_msgs::TwistStamped out_msg = *msg; + if(emergency_stop_ == false){ ++ // out_msg.twist.linear.x = twist_out.lx; ++ // out_msg.twist.angular.z = twist_out.az; + out_msg.twist.linear.x = twist_out.lx; + out_msg.twist.angular.z = twist_out.az; + } +@@ -121,15 +115,11 @@ inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConst + out_msg.twist.angular.z = 0; + } + twist_pub_.publish(out_msg); +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_msgs::TwistStamped rubis_out_msg; +- rubis_out_msg.instance = rubis::instance_; +- rubis_out_msg.msg = out_msg; +- rubis_twist_pub_.publish(rubis_out_msg); +- } +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ rubis_msgs::TwistStamped rubis_out_msg; ++ rubis_out_msg.instance = rubis::instance_; ++ rubis_out_msg.obj_instance = rubis::obj_instance_; ++ rubis_out_msg.msg = out_msg; ++ rubis_twist_pub_.publish(rubis_out_msg); + + // Publish lateral accel and jerk after smoothing + auto lacc_smoothed_result = twist_filter_ptr_->calcLaccWithAngularZ(twist_out); +@@ -153,20 +143,37 @@ inline void TwistFilterNode::publishTwist(const geometry_msgs::TwistStampedConst + } + + void TwistFilterNode::rubisTwistCmdCallback(const rubis_msgs::TwistStampedConstPtr& _msg){ ++ // Before spin ++ rubis::start_task_profiling(); ++ ++ // Callback ++ _emergencyStopCallback(); ++ _ctrlCmdCallback(ctrl_cmd_ptr_); ++ + geometry_msgs::TwistStampedConstPtr msg = boost::make_shared(_msg-> msg); + rubis::instance_ = _msg->instance; ++ rubis::obj_instance_ = _msg->obj_instance; + publishTwist(msg); ++ ++ // After spin ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + + void TwistFilterNode::twistCmdCallback(const geometry_msgs::TwistStampedConstPtr& msg) + { +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + publishTwist(msg); + } + + void TwistFilterNode::ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg) + { ++ ctrl_cmd_ptr_ = boost::make_shared(*msg); ++} ++ ++void TwistFilterNode::_ctrlCmdCallback(const autoware_msgs::ControlCommandStampedConstPtr& msg) ++{ ++ if(ctrl_cmd_ptr_ == NULL) return; + const twist_filter::Ctrl ctrl = { msg->cmd.linear_velocity, msg->cmd.steering_angle }; + ros::Time current_time = ros::Time::now(); + +@@ -236,15 +243,19 @@ void TwistFilterNode::ctrlCmdCallback(const autoware_msgs::ControlCommandStamped + } + + void TwistFilterNode::emergencyStopCallback(const std_msgs::Bool& msg){ +- bool current_emergency_stop = msg.data; ++ current_emergency_stop_ = msg.data; ++ return; ++} ++ ++void TwistFilterNode::_emergencyStopCallback(){ + static std::string state("none"); + +- if(current_emergency_stop == true){ ++ if(current_emergency_stop_ == true){ + state = std::string("object is detected"); + emergency_stop_ = true; + current_stop_count_ = max_stop_count_; + } +- else if(current_emergency_stop == false && emergency_stop_ == true){ // Emergency Stop event is finished or wait ++ else if(current_emergency_stop_ == false && emergency_stop_ == true){ // Emergency Stop event is finished or wait + current_stop_count_--; + if(current_stop_count_ > 0){ + state = std::string("Wait for go"); +@@ -253,7 +264,7 @@ void TwistFilterNode::emergencyStopCallback(const std_msgs::Bool& msg){ + else + emergency_stop_ = false; + } +- else if(current_emergency_stop == false && emergency_stop_ == false){ // No event ++ else if(current_emergency_stop_ == false && emergency_stop_ == false){ // No event + state = std::string("No object"); + emergency_stop_ = false; + current_stop_count_ = 0; +diff --git a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp +index eb82a63b..6a1da019 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp ++++ b/autoware.ai/src/autoware/core_planning/twist_filter/src/twist_filter_node_main.cpp +@@ -17,67 +17,38 @@ + #include "twist_filter/twist_filter_node.h" + + extern unsigned long rubis::instance_; ++extern + + int main(int argc, char** argv) + { + ros::init(argc, argv, "twist_filter"); + twist_filter_node::TwistFilterNode node; + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- + ros::NodeHandle private_nh("~"); +- private_nh.param("/twist_filter/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/twist_filter/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/twist_filter/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_filter.csv"); +- private_nh.param("/twist_filter/rate", rate, 10); +- private_nh.param("/twist_filter/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/twist_filter/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/twist_filter/task_relative_deadline", task_relative_deadline, (double)10); +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); + +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag){ +- rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- } ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); ++ std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_filter.csv"); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + + return 0; + } +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt b/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt +index 3073a8b7..6d0f645d 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt ++++ b/autoware.ai/src/autoware/core_planning/twist_gate/CMakeLists.txt +@@ -44,17 +44,4 @@ install( + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +-) +- +-if(CATKIN_ENABLE_TESTING) +- roslint_add_test() +- find_package(rostest REQUIRED) +- add_rostest_gtest( +- test-twist_gate +- test/test_twist_gate.test +- test/src/test_twist_gate.cpp +- src/twist_gate.cpp +- ) +- add_dependencies(test-twist_gate ${catkin_EXPORTED_TARGETS}) +- target_link_libraries(test-twist_gate ${catkin_LIBRARIES}) +-endif() ++) +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h b/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h +index ba9dae2c..b6254f84 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h ++++ b/autoware.ai/src/autoware/core_planning/twist_gate/include/twist_gate/twist_gate.h +@@ -60,7 +60,8 @@ private: + void watchdogTimer(); + void remoteCmdCallback(const remote_msgs_t::ConstPtr& input_msg); + void autoCmdTwistCmdCallback(const geometry_msgs::TwistStamped::ConstPtr& input_msg); +- void autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& _input_msg); ++ void autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg); ++ void _autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg); + void modeCmdCallback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg); + void gearCmdCallback(const tablet_socket_msgs::gear_cmd::ConstPtr& input_msg); + void accelCmdCallback(const autoware_msgs::AccelCmd::ConstPtr& input_msg); +@@ -89,6 +90,9 @@ private: + ros::Subscriber config_sub_; + std::map auto_cmd_sub_stdmap_; + ros::Timer timer_; ++ ros::Time current_time_; ++ ++ bool is_current_time_changed_; + + vehicle_cmd_msg_t twist_gate_msg_; + rubis_msgs::VehicleCmd rubis_twist_gate_msg_; +@@ -101,6 +105,8 @@ private: + std::thread watchdog_timer_thread_; + bool is_alive; + ++ rubis_msgs::TwistStamped::ConstPtr rubis_twist_cmd_ptr_; ++ + enum class CommandMode + { + AUTO = 1, +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp +index d008743b..e812acc9 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp ++++ b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate.cpp +@@ -45,32 +45,33 @@ TwistGate::TwistGate(const ros::NodeHandle& nh, const ros::NodeHandle& private_n + , timeout_period_(10.0) + , command_mode_(CommandMode::AUTO) + , previous_command_mode_(CommandMode::AUTO) ++ , rubis_twist_cmd_ptr_(NULL) ++ , current_time_(0) ++ , is_current_time_changed_(false) + { + private_nh_.param("loop_rate", loop_rate_, 30.0); + private_nh_.param("use_decision_maker", use_decision_maker_, false); +- private_nh_.param("instance_mode", rubis::instance_mode_, 0); + + control_command_pub_ = nh_.advertise("/ctrl_mode", 1); + vehicle_cmd_pub_ = nh_.advertise("/vehicle_cmd", 1, true); +- if(rubis::instance_mode_) rubis_vehicle_cmd_pub_ = nh_.advertise("/rubis_vehicle_cmd", 1, true); +- remote_cmd_sub_ = nh_.subscribe("/remote_cmd", 1, &TwistGate::remoteCmdCallback, this); +- config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistGate::configCallback, this); +- ++ rubis_vehicle_cmd_pub_ = nh_.advertise("/rubis_vehicle_cmd", 1, true); ++ + timer_ = nh_.createTimer(ros::Duration(1.0 / loop_rate_), &TwistGate::timerCallback, this); + +- if(rubis::instance_mode_) auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/rubis_twist_cmd", 1, &TwistGate::autoCmdRubisTwistCmdCallback, this); +- else auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/twist_cmd", 1, &TwistGate::autoCmdTwistCmdCallback, this); ++ // remote_cmd_sub_ = nh_.subscribe("/remote_cmd", 1, &TwistGate::remoteCmdCallback, this); ++ config_sub_ = nh_.subscribe("config/twist_filter", 1, &TwistGate::configCallback, this); ++ auto_cmd_sub_stdmap_["twist_cmd"] = nh_.subscribe("/rubis_twist_cmd", 1, &TwistGate::autoCmdRubisTwistCmdCallback, this); + +- auto_cmd_sub_stdmap_["mode_cmd"] = nh_.subscribe("/mode_cmd", 1, &TwistGate::modeCmdCallback, this); +- auto_cmd_sub_stdmap_["gear_cmd"] = nh_.subscribe("/gear_cmd", 1, &TwistGate::gearCmdCallback, this); +- auto_cmd_sub_stdmap_["accel_cmd"] = nh_.subscribe("/accel_cmd", 1, &TwistGate::accelCmdCallback, this); +- auto_cmd_sub_stdmap_["steer_cmd"] = nh_.subscribe("/steer_cmd", 1, &TwistGate::steerCmdCallback, this); +- auto_cmd_sub_stdmap_["brake_cmd"] = nh_.subscribe("/brake_cmd", 1, &TwistGate::brakeCmdCallback, this); +- auto_cmd_sub_stdmap_["lamp_cmd"] = nh_.subscribe("/lamp_cmd", 1, &TwistGate::lampCmdCallback, this); +- auto_cmd_sub_stdmap_["ctrl_cmd"] = nh_.subscribe("/ctrl_cmd", 1, &TwistGate::ctrlCmdCallback, this); +- auto_cmd_sub_stdmap_["state"] = nh_.subscribe("/decision_maker/state", 1, &TwistGate::stateCallback, this); +- auto_cmd_sub_stdmap_["emergency_velocity"] = +- nh_.subscribe("emergency_velocity", 1, &TwistGate::emergencyCmdCallback, this); ++ // auto_cmd_sub_stdmap_["mode_cmd"] = nh_.subscribe("/mode_cmd", 1, &TwistGate::modeCmdCallback, this); ++ // auto_cmd_sub_stdmap_["gear_cmd"] = nh_.subscribe("/gear_cmd", 1, &TwistGate::gearCmdCallback, this); ++ // auto_cmd_sub_stdmap_["accel_cmd"] = nh_.subscribe("/accel_cmd", 1, &TwistGate::accelCmdCallback, this); ++ // auto_cmd_sub_stdmap_["steer_cmd"] = nh_.subscribe("/steer_cmd", 1, &TwistGate::steerCmdCallback, this); ++ // auto_cmd_sub_stdmap_["brake_cmd"] = nh_.subscribe("/brake_cmd", 1, &TwistGate::brakeCmdCallback, this); ++ // auto_cmd_sub_stdmap_["lamp_cmd"] = nh_.subscribe("/lamp_cmd", 1, &TwistGate::lampCmdCallback, this); ++ // auto_cmd_sub_stdmap_["ctrl_cmd"] = nh_.subscribe("/ctrl_cmd", 1, &TwistGate::ctrlCmdCallback, this); ++ // auto_cmd_sub_stdmap_["state"] = nh_.subscribe("/decision_maker/state", 1, &TwistGate::stateCallback, this); ++ // auto_cmd_sub_stdmap_["emergency_velocity"] = ++ // nh_.subscribe("emergency_velocity", 1, &TwistGate::emergencyCmdCallback, this); + + + twist_gate_msg_.header.seq = 0; +@@ -192,15 +193,26 @@ void TwistGate::remoteCmdCallback(const remote_msgs_t::ConstPtr& input_msg) + + void TwistGate::autoCmdTwistCmdCallback(const geometry_msgs::TwistStamped::ConstPtr& input_msg) + { +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + updateTwistGateMsg(input_msg); + } + +-void TwistGate::autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& _input_msg) ++void TwistGate::_autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg) + { +- geometry_msgs::TwistStamped::ConstPtr input_msg = boost::make_shared(_input_msg->msg); +- rubis::instance_ = _input_msg->instance; +- updateTwistGateMsg(input_msg); ++ if(rubis_twist_cmd_ptr_ == NULL) return; ++ if(!is_current_time_changed_){ ++ current_time_ = input_msg->msg.header.stamp; ++ is_current_time_changed_ = true; ++ } ++ rubis::instance_ = input_msg->instance; ++ rubis::obj_instance_ = input_msg->obj_instance; ++ geometry_msgs::TwistStamped::ConstPtr _input_msg = boost::make_shared(input_msg->msg); ++ updateTwistGateMsg(_input_msg); ++} ++ ++void TwistGate::autoCmdRubisTwistCmdCallback(const rubis_msgs::TwistStamped::ConstPtr& input_msg) ++{ ++ rubis_twist_cmd_ptr_ = boost::make_shared(*input_msg); + } + + inline void TwistGate::updateTwistGateMsg(const geometry_msgs::TwistStamped::ConstPtr& input_msg){ +@@ -211,14 +223,9 @@ inline void TwistGate::updateTwistGateMsg(const geometry_msgs::TwistStamped::Con + twist_gate_msg_.header.seq++; + twist_gate_msg_.twist_cmd.twist = input_msg->twist; + +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_twist_gate_msg_.instance = rubis::instance_; +- } ++ rubis_twist_gate_msg_.instance = rubis::instance_; + checkState(); + } +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; + } + + void TwistGate::modeCmdCallback(const tablet_socket_msgs::mode_cmd::ConstPtr& input_msg) +@@ -347,15 +354,45 @@ void TwistGate::emergencyCmdCallback(const vehicle_cmd_msg_t::ConstPtr& input_ms + + void TwistGate::timerCallback(const ros::TimerEvent& e) + { ++ rubis::start_task_profiling(); ++ if(rubis_twist_cmd_ptr_ == NULL){ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ return; ++ } ++ ++ ++ static ros::Time previous_time = ros::Time::now(); ++ static double previous_target_velocity = 0.0; ++ static double previous_target_accel = 0.0; ++ ++ // Callback ++ _autoCmdRubisTwistCmdCallback(rubis_twist_cmd_ptr_); ++ ++ double diff_time = (current_time_ - previous_time).toSec(); ++ double current_target_velocity = 0.0, current_target_accel = 0.0; ++ ++ current_target_velocity = rubis_twist_cmd_ptr_->msg.twist.linear.x; ++ current_target_accel = (diff_time > 0) ? ((current_target_velocity - previous_target_velocity) / diff_time) : previous_target_accel; ++ + if(zero_flag_ == 1) + resetVehicleCmdMsg(); ++ twist_gate_msg_.ctrl_cmd.linear_acceleration = current_target_accel; ++ twist_gate_msg_.ctrl_cmd.linear_velocity = current_target_velocity; + + vehicle_cmd_pub_.publish(twist_gate_msg_); +- if(rubis::instance_mode_){ +- rubis_twist_gate_msg_.msg = twist_gate_msg_; +- rubis_vehicle_cmd_pub_.publish(rubis_twist_gate_msg_); ++ rubis_twist_gate_msg_.msg = twist_gate_msg_; ++ rubis_vehicle_cmd_pub_.publish(rubis_twist_gate_msg_); ++ ++ ++ ++ if(is_current_time_changed_){ ++ previous_time = current_time_; ++ previous_target_velocity = current_target_velocity; ++ previous_target_accel = current_target_accel; ++ is_current_time_changed_ = false; + } +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); + } + + void TwistGate::configCallback(const autoware_config_msgs::ConfigTwistFilter& msg) +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp +index e3f3e429..9e0bc827 100644 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp ++++ b/autoware.ai/src/autoware/core_planning/twist_gate/src/twist_gate_node.cpp +@@ -28,61 +28,31 @@ int main(int argc, char** argv) + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; +- std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; +- +- private_nh.param("/twist_gate/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/twist_gate/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/twist_gate/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_gate.csv"); +- private_nh.param("/twist_gate/rate", rate, 10); +- private_nh.param("/twist_gate/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/twist_gate/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/twist_gate/task_relative_deadline", task_relative_deadline, (double)10); +- private_nh.param("/twist_gate/zero_flag", zero_flag_, 0); +- + TwistGate twist_gate(nh, private_nh); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); ++ std::string task_response_time_filename; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/twist_gate.csv"); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + + return 0; + } +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp b/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp +deleted file mode 100644 +index cf8882a2..00000000 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.cpp ++++ /dev/null +@@ -1,351 +0,0 @@ +-/* +- * Copyright 2019 Autoware Foundation. All rights reserved. +- * +- * Licensed under the Apache License, Version 2.0 (the "License"); +- * you may not use this file except in compliance with the License. +- * You may obtain a copy of the License at +- * +- * http://www.apache.org/licenses/LICENSE-2.0 +- * +- * Unless required by applicable law or agreed to in writing, software +- * distributed under the License is distributed on an "AS IS" BASIS, +- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +- * See the License for the specific language governing permissions and +- * limitations under the License. +- */ +- +-#include +-#include +- +-#include "test_twist_gate.hpp" +- +-class TwistGateTestSuite : public ::testing::Test +-{ +-public: +- TwistGateTestSuite() {} +- ~TwistGateTestSuite() {} +- +- TwistGateTestClass test_obj_; +- +-protected: +- virtual void SetUp() +- { +- ros::NodeHandle nh; +- ros::NodeHandle private_nh("~"); +- test_obj_.tg = new TwistGate(nh, private_nh); +- }; +- virtual void TearDown() { delete test_obj_.tg; } +-}; +- +-TEST_F(TwistGateTestSuite, resetVehicelCmd) +-{ +- double d_value = 1.5; +- int i_value = 1; +- +- autoware_msgs::VehicleCmd msg = test_obj_.setTgTwistGateMsg(d_value, i_value); +- +- ASSERT_EQ(d_value, msg.twist_cmd.twist.linear.x); +- ASSERT_EQ(d_value, msg.twist_cmd.twist.angular.z); +- ASSERT_EQ(i_value, msg.mode); +- ASSERT_EQ(i_value, msg.gear_cmd.gear); +- ASSERT_EQ(i_value, msg.lamp_cmd.l); +- ASSERT_EQ(i_value, msg.lamp_cmd.r); +- ASSERT_EQ(i_value, msg.accel_cmd.accel); +- ASSERT_EQ(i_value, msg.brake_cmd.brake); +- ASSERT_EQ(i_value, msg.steer_cmd.steer); +- ASSERT_EQ(i_value, msg.ctrl_cmd.linear_velocity); +- ASSERT_EQ(i_value, msg.ctrl_cmd.steering_angle); +- ASSERT_EQ(i_value, msg.emergency); +- +- test_obj_.tgResetVehicleCmdMsg(); +- msg = test_obj_.getTgTwistGateMsg(); +- +- ASSERT_EQ(0, msg.twist_cmd.twist.linear.x); +- ASSERT_EQ(0, msg.twist_cmd.twist.angular.z); +- ASSERT_EQ(0, msg.mode); +- ASSERT_EQ(0, msg.gear_cmd.gear); +- ASSERT_EQ(0, msg.lamp_cmd.l); +- ASSERT_EQ(0, msg.lamp_cmd.r); +- ASSERT_EQ(0, msg.accel_cmd.accel); +- ASSERT_EQ(0, msg.brake_cmd.brake); +- ASSERT_EQ(0, msg.steer_cmd.steer); +- ASSERT_EQ(-1, msg.ctrl_cmd.linear_velocity); +- ASSERT_EQ(0, msg.ctrl_cmd.steering_angle); +- ASSERT_EQ(0, msg.emergency); +-} +- +-TEST_F(TwistGateTestSuite, twistCmdCallback) +-{ +- double linear_x = 5.0; +- double angular_z = 1.5; +- test_obj_.publishTwistCmd(linear_x, angular_z); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(linear_x, test_obj_.cb_vehicle_cmd.twist_cmd.twist.linear.x); +- ASSERT_EQ(angular_z, test_obj_.cb_vehicle_cmd.twist_cmd.twist.angular.z); +-} +- +-TEST_F(TwistGateTestSuite, controlCmdCallback) +-{ +- double linear_vel = 5.0; +- double linear_acc = 1.5; +- double steer_angle = 1.57; +- test_obj_.publishControlCmd(linear_vel, linear_acc, steer_angle); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(linear_vel, test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_velocity); +- ASSERT_EQ(linear_acc, test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_acceleration); +- ASSERT_EQ(steer_angle, test_obj_.cb_vehicle_cmd.ctrl_cmd.steering_angle); +-} +- +-TEST_F(TwistGateTestSuite, remoteCmdCallback) +-{ +- autoware_msgs::RemoteCmd remote_cmd; +- remote_cmd.vehicle_cmd.header.frame_id = "/test_frame"; +- remote_cmd.vehicle_cmd.header.stamp = ros::Time::now(); +- remote_cmd.vehicle_cmd.twist_cmd.twist.linear.x = 5.0; +- remote_cmd.vehicle_cmd.twist_cmd.twist.angular.z = 0.785; +- remote_cmd.vehicle_cmd.ctrl_cmd.linear_velocity = 4.0; +- remote_cmd.vehicle_cmd.ctrl_cmd.linear_acceleration = 3.0; +- remote_cmd.vehicle_cmd.ctrl_cmd.steering_angle = 0.393; +- remote_cmd.vehicle_cmd.accel_cmd.accel = 10; +- remote_cmd.vehicle_cmd.brake_cmd.brake = 20; +- remote_cmd.vehicle_cmd.steer_cmd.steer = 30; +- remote_cmd.vehicle_cmd.gear_cmd.gear = autoware_msgs::Gear::PARK; +- remote_cmd.vehicle_cmd.lamp_cmd.l = 1; +- remote_cmd.vehicle_cmd.lamp_cmd.r = 1; +- remote_cmd.vehicle_cmd.mode = 6; +- remote_cmd.vehicle_cmd.emergency = 0; +- remote_cmd.control_mode = 2; +- +- test_obj_.publishRemoteCmd(remote_cmd); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(remote_cmd.vehicle_cmd.header.frame_id +- , test_obj_.cb_vehicle_cmd.header.frame_id); +- ASSERT_EQ(remote_cmd.vehicle_cmd.header.stamp +- , test_obj_.cb_vehicle_cmd.header.stamp); +- ASSERT_EQ(remote_cmd.vehicle_cmd.twist_cmd.twist.linear.x +- , test_obj_.cb_vehicle_cmd.twist_cmd.twist.linear.x); +- ASSERT_EQ(remote_cmd.vehicle_cmd.twist_cmd.twist.angular.z +- , test_obj_.cb_vehicle_cmd.twist_cmd.twist.angular.z); +- ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.linear_velocity +- , test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_velocity); +- ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.linear_acceleration +- , test_obj_.cb_vehicle_cmd.ctrl_cmd.linear_acceleration); +- ASSERT_EQ(remote_cmd.vehicle_cmd.ctrl_cmd.steering_angle +- , test_obj_.cb_vehicle_cmd.ctrl_cmd.steering_angle); +- ASSERT_EQ(remote_cmd.vehicle_cmd.accel_cmd.accel +- , test_obj_.cb_vehicle_cmd.accel_cmd.accel); +- ASSERT_EQ(remote_cmd.vehicle_cmd.brake_cmd.brake +- , test_obj_.cb_vehicle_cmd.brake_cmd.brake); +- ASSERT_EQ(remote_cmd.vehicle_cmd.steer_cmd.steer +- , test_obj_.cb_vehicle_cmd.steer_cmd.steer); +- ASSERT_EQ(remote_cmd.vehicle_cmd.gear_cmd.gear +- , test_obj_.cb_vehicle_cmd.gear_cmd.gear); +- ASSERT_EQ(remote_cmd.vehicle_cmd.lamp_cmd.l +- , test_obj_.cb_vehicle_cmd.lamp_cmd.l); +- ASSERT_EQ(remote_cmd.vehicle_cmd.lamp_cmd.r +- , test_obj_.cb_vehicle_cmd.lamp_cmd.r); +- ASSERT_EQ(remote_cmd.vehicle_cmd.mode +- , test_obj_.cb_vehicle_cmd.mode); +-} +- +-TEST_F(TwistGateTestSuite, modeCmdCallback) +-{ +- int mode = 8; +- +- test_obj_.publishModeCmd(mode); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(mode, test_obj_.cb_vehicle_cmd.mode); +-} +- +-TEST_F(TwistGateTestSuite, gearCmdCallback) +-{ +- int gear = autoware_msgs::Gear::DRIVE; +- +- test_obj_.publishGearCmd(gear); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(gear, test_obj_.cb_vehicle_cmd.gear_cmd.gear); +-} +- +-TEST_F(TwistGateTestSuite, accelCmdCallback) +-{ +- int accel = 100; +- +- test_obj_.publishAccelCmd(accel); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(accel, test_obj_.cb_vehicle_cmd.accel_cmd.accel); +-} +- +-TEST_F(TwistGateTestSuite, steerCmdCallback) +-{ +- int steer = 100; +- +- test_obj_.publishSteerCmd(steer); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(steer, test_obj_.cb_vehicle_cmd.steer_cmd.steer); +-} +- +-TEST_F(TwistGateTestSuite, brakeCmdCallback) +-{ +- int brake = 100; +- +- test_obj_.publishBrakeCmd(brake); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- +- ASSERT_EQ(brake, test_obj_.cb_vehicle_cmd.brake_cmd.brake); +-} +- +-TEST_F(TwistGateTestSuite, lampCmdCallback) +-{ +- int lamp_l = 1; +- int lamp_r = 1; +- +- test_obj_.publishLampCmd(lamp_l, lamp_r); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(lamp_l, test_obj_.cb_vehicle_cmd.lamp_cmd.l); +- ASSERT_EQ(lamp_r, test_obj_.cb_vehicle_cmd.lamp_cmd.r); +-} +- +-TEST_F(TwistGateTestSuite, emergencyVehicleCmdCallback) +-{ +- int lamp_l_pre = 0; +- int lamp_r_pre = 0; +- int emergency = 1; +- +- test_obj_.publishLampCmd(lamp_l_pre, lamp_r_pre); +- test_obj_.publishEmergencyVehicleCmd(emergency); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(emergency, test_obj_.cb_vehicle_cmd.emergency); +- +- int lamp_l = 1; +- int lamp_r = 1; +- test_obj_.publishLampCmd(lamp_l, lamp_r); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(lamp_l_pre, test_obj_.cb_vehicle_cmd.lamp_cmd.l); +- ASSERT_EQ(lamp_r_pre, test_obj_.cb_vehicle_cmd.lamp_cmd.r); +- +- std::this_thread::sleep_for(std::chrono::milliseconds(10000)); +- +- test_obj_.publishLampCmd(lamp_l, lamp_r); +- ros::WallDuration(0.1).sleep(); +- test_obj_.tgSpinOnce(); +- +- for (int i = 0; i < 3; i++) +- { +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- } +- +- ASSERT_EQ(lamp_l, test_obj_.cb_vehicle_cmd.lamp_cmd.l); +- ASSERT_EQ(lamp_r, test_obj_.cb_vehicle_cmd.lamp_cmd.r); +-} +- +-TEST_F(TwistGateTestSuite, stateCallback) +-{ +- test_obj_.publishDecisionMakerState("VehicleReady\nWaitOrder\nWaitEngage\n"); +- test_obj_.tgSpinOnce(); +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- +- autoware_msgs::VehicleCmd tg_msg = test_obj_.getTwistGateMsg(); +- ASSERT_EQ(autoware_msgs::Gear::PARK, tg_msg.gear_cmd.gear); +- ASSERT_EQ(false, test_obj_.getIsStateDriveFlag()); +- +- test_obj_.publishDecisionMakerState("VehicleReady\nDriving\nDrive\n"); +- test_obj_.tgSpinOnce(); +- ros::WallDuration(0.1).sleep(); +- ros::spinOnce(); +- +- tg_msg = test_obj_.getTwistGateMsg(); +- ASSERT_EQ(autoware_msgs::Gear::DRIVE, tg_msg.gear_cmd.gear); +- ASSERT_EQ(true, test_obj_.getIsStateDriveFlag()); +-} +- +-int main(int argc, char **argv) +-{ +- testing::InitGoogleTest(&argc, argv); +- ros::init(argc, argv, "TwistGateTestNode"); +- return RUN_ALL_TESTS(); +-} +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp b/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp +deleted file mode 100644 +index bd4acca1..00000000 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/src/test_twist_gate.hpp ++++ /dev/null +@@ -1,168 +0,0 @@ +-/* +- * Copyright 2019 Autoware Foundation. All rights reserved. +- * +- * Licensed under the Apache License, Version 2.0 (the "License"); +- * you may not use this file except in compliance with the License. +- * You may obtain a copy of the License at +- * +- * http://www.apache.org/licenses/LICENSE-2.0 +- * +- * Unless required by applicable law or agreed to in writing, software +- * distributed under the License is distributed on an "AS IS" BASIS, +- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +- * See the License for the specific language governing permissions and +- * limitations under the License. +- */ +- +-#include +-#include +- +-#include "twist_gate/twist_gate.h" +- +-class TwistGateTestClass { +-public: +- TwistGateTestClass() { +- twist_cmd_publisher = +- nh.advertise("twist_cmd", 0); +- control_cmd_publisher = +- nh.advertise("ctrl_cmd", 0); +- decision_maker_state_publisher = +- nh.advertise("decision_maker/state", 0); +- remote_cmd_publisher = nh.advertise("remote_cmd", 0); +- mode_cmd_publisher = nh.advertise("mode_cmd", 0); +- gear_cmd_publisher = nh.advertise("gear_cmd", 0); +- accel_cmd_publisher = nh.advertise("accel_cmd", 0); +- steer_cmd_publisher = nh.advertise("steer_cmd", 0); +- brake_cmd_publisher = nh.advertise("brake_cmd", 0); +- lamp_cmd_publisher = nh.advertise("lamp_cmd", 0); +- emergency_vehicle_cmd_publisher = nh.advertise("emergency_velocity", 0); +- vehicle_cmd_subscriber = nh.subscribe( +- "/vehicle_cmd", 1, &TwistGateTestClass::vehicleCmdCallback, this); +- } +- +- TwistGate *tg; +- autoware_msgs::VehicleCmd cb_vehicle_cmd; +- +- ros::NodeHandle nh; +- ros::Publisher twist_cmd_publisher, control_cmd_publisher, remote_cmd_publisher, mode_cmd_publisher, gear_cmd_publisher, accel_cmd_publisher, steer_cmd_publisher, brake_cmd_publisher, lamp_cmd_publisher, decision_maker_state_publisher, emergency_vehicle_cmd_publisher; +- ros::Subscriber vehicle_cmd_subscriber; +- +- void tgSpinOnce() { tg->spinOnce(); } +- +- void tgResetVehicleCmdMsg() { tg->resetVehicleCmdMsg(); } +- +- autoware_msgs::VehicleCmd setTgTwistGateMsg(double d_value, int i_value) { +- tg->twist_gate_msg_.twist_cmd.twist.linear.x = d_value; +- tg->twist_gate_msg_.twist_cmd.twist.angular.z = d_value; +- tg->twist_gate_msg_.mode = i_value; +- tg->twist_gate_msg_.gear_cmd.gear = i_value; +- tg->twist_gate_msg_.lamp_cmd.l = i_value; +- tg->twist_gate_msg_.lamp_cmd.r = i_value; +- tg->twist_gate_msg_.accel_cmd.accel = i_value; +- tg->twist_gate_msg_.brake_cmd.brake = i_value; +- tg->twist_gate_msg_.steer_cmd.steer = i_value; +- tg->twist_gate_msg_.ctrl_cmd.linear_velocity = i_value; +- tg->twist_gate_msg_.ctrl_cmd.steering_angle = i_value; +- tg->twist_gate_msg_.emergency = i_value; +- +- return tg->twist_gate_msg_; +- } +- +- autoware_msgs::VehicleCmd getTgTwistGateMsg() {return tg->twist_gate_msg_;} +- +- void publishTwistCmd(double linear_x, double angular_z) { +- geometry_msgs::TwistStamped msg; +- msg.header.stamp = ros::Time::now(); +- msg.twist.linear.x = linear_x; +- msg.twist.angular.z = angular_z; +- +- twist_cmd_publisher.publish(msg); +- } +- +- void publishControlCmd(double linear_vel, double linear_acc, +- double steer_angle) { +- autoware_msgs::ControlCommandStamped msg; +- msg.header.stamp = ros::Time::now(); +- msg.cmd.linear_velocity = linear_vel; +- msg.cmd.linear_acceleration = linear_acc; +- msg.cmd.steering_angle = steer_angle; +- +- control_cmd_publisher.publish(msg); +- } +- +- void publishRemoteCmd(autoware_msgs::RemoteCmd remote_cmd){ +- remote_cmd_publisher.publish(remote_cmd); +- } +- +- void publishModeCmd(int mode){ +- tablet_socket_msgs::mode_cmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.mode = mode; +- +- mode_cmd_publisher.publish(msg); +- } +- +- void publishGearCmd(int gear){ +- tablet_socket_msgs::gear_cmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.gear = gear; +- +- gear_cmd_publisher.publish(msg); +- } +- +- void publishAccelCmd(int accel){ +- autoware_msgs::AccelCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.accel = accel; +- +- accel_cmd_publisher.publish(msg); +- } +- +- void publishSteerCmd(int steer){ +- autoware_msgs::SteerCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.steer = steer; +- +- steer_cmd_publisher.publish(msg); +- } +- +- void publishBrakeCmd(int brake){ +- autoware_msgs::BrakeCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.brake = brake; +- +- brake_cmd_publisher.publish(msg); +- } +- +- void publishLampCmd(int lamp_l, int lamp_r){ +- autoware_msgs::LampCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.l = lamp_l; +- msg.r = lamp_r; +- +- lamp_cmd_publisher.publish(msg); +- } +- +- void publishEmergencyVehicleCmd(int emergency){ +- autoware_msgs::VehicleCmd msg; +- msg.header.stamp = ros::Time::now(); +- msg.emergency = emergency; +- +- emergency_vehicle_cmd_publisher.publish(msg); +- } +- +- void publishDecisionMakerState(std::string states) { +- std_msgs::String msg; +- msg.data = states; +- +- decision_maker_state_publisher.publish(msg); +- } +- +- void vehicleCmdCallback(autoware_msgs::VehicleCmd msg) { +- cb_vehicle_cmd = msg; +- } +- +- autoware_msgs::VehicleCmd getTwistGateMsg() { return tg->twist_gate_msg_; } +- +- bool getIsStateDriveFlag() { return tg->is_state_drive_; } +-}; +diff --git a/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test b/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test +deleted file mode 100644 +index abe4934a..00000000 +--- a/autoware.ai/src/autoware/core_planning/twist_gate/test/test_twist_gate.test ++++ /dev/null +@@ -1,5 +0,0 @@ +- +- +- +- +- +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst b/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst +new file mode 100644 +index 00000000..41c70f27 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/CHANGELOG.rst +@@ -0,0 +1,267 @@ ++^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ++Changelog for package nav_msgs ++^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ++ ++1.13.1 (2021-01-11) ++------------------- ++* Update package maintainers (`#168 `_) ++* Add LoadMap service (`#164 `_) ++* Contributors: David V. Lu!!, Michel Hidalgo ++ ++1.13.0 (2020-05-21) ++------------------- ++* Bump CMake version to avoid CMP0048 warning (`#158 `_) ++* Contributors: Shane Loretz ++ ++1.12.7 (2018-11-06) ++------------------- ++ ++1.12.6 (2018-05-03) ++------------------- ++ ++1.12.5 (2016-09-30) ++------------------- ++ ++1.12.4 (2016-02-22) ++------------------- ++ ++1.12.3 (2015-04-20) ++------------------- ++ ++1.12.2 (2015-03-21) ++------------------- ++* change type of initial_pose in SetMap service to PoseWithCovarianceStamped ++* Contributors: Stephan Wirth ++ ++1.12.1 (2015-03-17) ++------------------- ++* updating outdated urls. fixes `#52 `_. ++* Adds a SetMap service message to support swap maps functionality in amcl ++* Contributors: Tully Foote, liz-murphy ++ ++1.12.0 (2014-12-29) ++------------------- ++ ++1.11.6 (2014-11-04) ++------------------- ++ ++1.11.5 (2014-10-27) ++------------------- ++ ++1.11.4 (2014-06-19) ++------------------- ++ ++1.11.3 (2014-05-07) ++------------------- ++* Export architecture_independent flag in package.xml ++* Contributors: Scott K Logan ++ ++1.11.2 (2014-04-24) ++------------------- ++ ++1.11.1 (2014-04-16) ++------------------- ++ ++1.11.0 (2014-03-04) ++------------------- ++ ++1.10.6 (2014-02-27) ++------------------- ++ ++1.10.5 (2014-02-25) ++------------------- ++ ++1.10.4 (2014-02-18) ++------------------- ++ ++1.10.3 (2014-01-07) ++------------------- ++ ++1.10.2 (2013-08-19) ++------------------- ++ ++1.10.1 (2013-08-16) ++------------------- ++ ++1.10.0 (2013-07-13) ++------------------- ++ ++1.9.16 (2013-05-21) ++------------------- ++* added action definition for getting maps ++* update email in package.xml ++ ++1.9.15 (2013-03-08) ++------------------- ++ ++1.9.14 (2013-01-19) ++------------------- ++ ++1.9.13 (2013-01-13) ++------------------- ++ ++1.9.12 (2013-01-02) ++------------------- ++ ++1.9.11 (2012-12-17) ++------------------- ++* modified dep type of catkin ++ ++1.9.10 (2012-12-13) ++------------------- ++* add missing downstream depend ++* switched from langs to message_* packages ++ ++1.9.9 (2012-11-22) ++------------------ ++ ++1.9.8 (2012-11-14) ++------------------ ++ ++1.9.7 (2012-10-30) ++------------------ ++* fix catkin function order ++ ++1.9.6 (2012-10-18) ++------------------ ++* updated cmake min version to 2.8.3, use cmake_parse_arguments instead of custom macro ++ ++1.9.5 (2012-09-28) ++------------------ ++ ++1.9.4 (2012-09-27 18:06) ++------------------------ ++ ++1.9.3 (2012-09-27 17:39) ++------------------------ ++* cleanup ++* cleaned up package.xml files ++* updated to latest catkin ++* fixed dependencies and more ++* updated to latest catkin: created package.xmls, updated CmakeLists.txt ++ ++1.9.2 (2012-09-05) ++------------------ ++* updated pkg-config in manifest.xml ++ ++1.9.1 (2012-09-04) ++------------------ ++* use install destination variables, removed manual installation of manifests ++ ++1.9.0 (2012-08-29) ++------------------ ++* updated to current catkin ++ ++1.8.13 (2012-07-26 18:34:15 +0000) ++---------------------------------- ++ ++1.8.8 (2012-06-12 22:36) ++------------------------ ++* make find_package REQUIRED ++* removed obsolete catkin tag from manifest files ++* fixed package dependencies for several common messages (fixed `#3956 `_) ++* adding manifest exports ++* removed depend, added catkin ++* stripping depend and export tags from common_msgs manifests as msg dependencies are now declared in cmake and stack.yaml. Also removed bag migration exports ++* common_msgs: removing migration rules as all are over a year old ++* bye bye vestigial MSG_DIRS ++* nav_msgs: getting rid of other build files and cleaning up ++* common_msgs: starting catkin conversion ++* adios rosbuild2 in manifest.xml ++* catkin updates ++* catkin_project ++* Updated to work with new message generation macros ++* More tweaking for standalone message generation ++* Getting standalone message generation working... w/o munging rosbuild2 ++* more rosbuild2 hacking ++* missing dependencies ++* updating bagmigration exports ++* rosbuild2 taking shape ++* removing old exports ros`#2292 `_ ++* Added Ubuntu platform tags to manifest ++* Adding a start pose to the GetPlan service ++* Remove use of deprecated rosbuild macros ++* Fixing migration rules for nav_msgs. ++* Changed byte to int8, in response to map_server doc review ++* changing review status ++* adding documentation for `#2997 `_ ++* removing redundant range statements as per ticket:2997 ++* Adding documentation to the Odometry message to make things more clear ++* manifest update ++* updated description and url ++* full migration rules ++* adding child_frame_id as per discussion about odometry message ++* Adding a header to Path ++* Adding a header to the GridCells message ++* Adding a new GridCells message for displaying obstacles in nav_view and rviz ++* clearing API reviews for they've been through a bunch of them recently. ++* fixing stack name ++* Adding comments to path ++* documenting messages ++* Making odometry migration fail until we have worked out appropriate way to handle covariances. ++* Changing naming of bag migration rules. ++* Modifying migration rules for Odometry and WrenchStamped change of field names. ++* Adding actual migration rules for all of the tested common_msgs migrations. ++* `#2250 `_ getting rid of _with_covariance in Odometry fields ++* nav_msgs: added missing srv export ++* Adding migration rules to get migration tests to pass. ++* removing last of robot_msgs and all dependencies on it ++* moving Path from robot_msgs to nav_msgs `#2281 `_ ++* adding header to OccupancyGrid `#1906 `_ ++* First half of the change from deprecated_msgs::RobotBase2DOdom to nav_msgs::Odometry, I think all the c++ compiles, can't speak for functionality yet, also... the python has yet to be run... this may break some things ++* moving PoseArray into geometry_msgs `#1907 `_ ++* fixing names ++* Removing header since there's already one in the pose and fixing message definition to have variable names ++* adding Odometry message as per API review and ticket:2250 ++* merging in the changes to messages see ros-users email. THis is about half the common_msgs API changes ++* Forgot to check in the services I added.... shoot ++* Moving StaticMap.srv to GetMap.srv and moving Plan.srv to GetPlan.srv, also moving them to nav_msgs and removing the nav_srvs package ++* Merging tha actionlib branch back into trunk ++ r29135@att (orig r19792): eitanme | 2009-07-27 18:30:30 -0700 ++ Creating a branch for actionlib.... hopefully for the last time ++ r29137@att (orig r19794): eitanme | 2009-07-27 18:32:49 -0700 ++ Changing ParticleCloud to PoseArray ++ r29139@att (orig r19796): eitanme | 2009-07-27 18:33:42 -0700 ++ Adding action definition to the rep ++ r29148@att (orig r19805): eitanme | 2009-07-27 18:47:39 -0700 ++ Some fixes... almost compiling ++ r29165@att (orig r19822): eitanme | 2009-07-27 20:41:07 -0700 ++ Macro version of the typedefs that compiles ++ r29213@att (orig r19869): eitanme | 2009-07-28 11:49:10 -0700 ++ Compling version of the ActionServer re-write complete with garbage collection, be default it will keep goals without goal handles for 5 seconds ++ r29220@att (orig r19876): eitanme | 2009-07-28 12:06:06 -0700 ++ Fix to make sure that transitions into preempting and recalling states actually happen ++ r29254@att (orig r19888): eitanme | 2009-07-28 13:27:40 -0700 ++ Forgot to actually call the cancel callback... addind a subscriber on the cancel topic ++ r29267@att (orig r19901): eitanme | 2009-07-28 14:41:09 -0700 ++ Adding text field to GoalStatus to allow users to throw some debugging information into the GoalStatus messages ++ r29275@att (orig r19909): eitanme | 2009-07-28 15:43:49 -0700 ++ Using tf remapping as I should've been doing for awhile ++ r29277@att (orig r19911): eitanme | 2009-07-28 15:48:48 -0700 ++ The navigation stack can now handle goals that aren't in the global frame. However, these goals will be transformed to the global frame at the time of reception, so for achieving them accurately the global frame of move_base should really be set to match the goals. ++ r29299@att (orig r19933): stuglaser | 2009-07-28 17:08:10 -0700 ++ Created genaction.py script to create the various messages that an action needs ++ r29376@att (orig r20003): vijaypradeep | 2009-07-29 02:45:24 -0700 ++ ActionClient is running. MoveBase ActionServer seems to be crashing ++ r29409@att (orig r20033): vijaypradeep | 2009-07-29 11:57:54 -0700 ++ Fixing bug with adding status trackers ++ r29410@att (orig r20034): vijaypradeep | 2009-07-29 11:58:18 -0700 ++ Changing from Release to Debug ++ r29432@att (orig r20056): vijaypradeep | 2009-07-29 14:07:30 -0700 ++ No longer building goal_manager_test.cpp ++ r29472@att (orig r20090): vijaypradeep | 2009-07-29 17:04:14 -0700 ++ Lots of Client-Side doxygen ++ r29484@att (orig r20101): vijaypradeep | 2009-07-29 18:35:01 -0700 ++ Adding to mainpage.dox ++ r29487@att (orig r20104): eitanme | 2009-07-29 18:55:06 -0700 ++ Removing file to help resolve merge I hope ++ r29489@att (orig r20106): eitanme | 2009-07-29 19:00:07 -0700 ++ Removing another file to try to resolve the branch ++ r29492@att (orig r20108): eitanme | 2009-07-29 19:14:25 -0700 ++ Again removing a file to get the merge working ++ r29493@att (orig r20109): eitanme | 2009-07-29 19:34:45 -0700 ++ Removing yet another file on which ssl negotiation fails ++ r29500@att (orig r20116): eitanme | 2009-07-29 19:54:18 -0700 ++ Fixing bug in genaction ++* moving MapMetaData and OccGrid into nav_msgs `#1303 `_ ++* created nav_msgs and moved ParticleCloud there `#1300 `_ +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt b/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt +new file mode 100644 +index 00000000..b4751819 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/CMakeLists.txt +@@ -0,0 +1,29 @@ ++cmake_minimum_required(VERSION 3.0.2) ++project(nav_msgs) ++ ++find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs actionlib_msgs) ++ ++add_message_files( ++ DIRECTORY msg ++ FILES ++ GridCells.msg ++ MapMetaData.msg ++ OccupancyGrid.msg ++ Odometry.msg ++ Path.msg) ++ ++add_service_files( ++ DIRECTORY srv ++ FILES ++ GetMap.srv ++ GetPlan.srv ++ SetMap.srv ++ LoadMap.srv) ++ ++add_action_files( ++ FILES ++ GetMap.action) ++ ++generate_messages(DEPENDENCIES geometry_msgs std_msgs actionlib_msgs) ++ ++catkin_package(CATKIN_DEPENDS geometry_msgs message_runtime std_msgs actionlib_msgs) +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action b/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action +new file mode 100644 +index 00000000..080e54fe +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/action/GetMap.action +@@ -0,0 +1,5 @@ ++# Get the map as a nav_msgs/OccupancyGrid ++--- ++nav_msgs/OccupancyGrid map ++--- ++# no feedback +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg +new file mode 100644 +index 00000000..2c928941 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/GridCells.msg +@@ -0,0 +1,5 @@ ++#an array of cells in a 2D grid ++Header header ++float32 cell_width ++float32 cell_height ++geometry_msgs/Point[] cells +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg +new file mode 100644 +index 00000000..398fbdd0 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/MapMetaData.msg +@@ -0,0 +1,13 @@ ++# This hold basic information about the characterists of the OccupancyGrid ++ ++# The time at which the map was loaded ++time map_load_time ++# The map resolution [m/cell] ++float32 resolution ++# Map width [cells] ++uint32 width ++# Map height [cells] ++uint32 height ++# The origin of the map [m, m, rad]. This is the real-world pose of the ++# cell (0,0) in the map. ++geometry_msgs/Pose origin +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg +new file mode 100644 +index 00000000..f2e79bda +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/OccupancyGrid.msg +@@ -0,0 +1,11 @@ ++# This represents a 2-D grid map, in which each cell represents the probability of ++# occupancy. ++ ++Header header ++ ++#MetaData for the map ++MapMetaData info ++ ++# The map data, in row-major order, starting with (0,0). Occupancy ++# probabilities are in the range [0,100]. Unknown is -1. ++int8[] data +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg +new file mode 100644 +index 00000000..73578ed8 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/Odometry.msg +@@ -0,0 +1,7 @@ ++# This represents an estimate of a position and velocity in free space. ++# The pose in this message should be specified in the coordinate frame given by header.frame_id. ++# The twist in this message should be specified in the coordinate frame given by the child_frame_id ++Header header ++string child_frame_id ++geometry_msgs/PoseWithCovariance pose ++geometry_msgs/TwistWithCovariance twist +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg b/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg +new file mode 100644 +index 00000000..979cb7d3 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/msg/Path.msg +@@ -0,0 +1,3 @@ ++#An array of poses that represents a Path for a robot to follow ++Header header ++geometry_msgs/PoseStamped[] poses +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/package.xml b/autoware.ai/src/autoware/messages/nav_msgs/package.xml +new file mode 100644 +index 00000000..f379061f +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/package.xml +@@ -0,0 +1,29 @@ ++ ++ nav_msgs ++ 1.13.1 ++ ++ nav_msgs defines the common messages used to interact with the ++ navigation stack. ++ ++ Michel Hidalgo ++ BSD ++ ++ http://wiki.ros.org/nav_msgs ++ Tully Foote ++ ++ catkin ++ ++ geometry_msgs ++ message_generation ++ std_msgs ++ actionlib_msgs ++ ++ geometry_msgs ++ message_runtime ++ std_msgs ++ actionlib_msgs ++ ++ ++ ++ ++ +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv +new file mode 100644 +index 00000000..6bd8e4a6 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetMap.srv +@@ -0,0 +1,3 @@ ++# Get the map as a nav_msgs/OccupancyGrid ++--- ++nav_msgs/OccupancyGrid map +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv +new file mode 100644 +index 00000000..f5c23edb +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/GetPlan.srv +@@ -0,0 +1,13 @@ ++# Get a plan from the current position to the goal Pose ++ ++# The start pose for the plan ++geometry_msgs/PoseStamped start ++ ++# The final pose of the goal position ++geometry_msgs/PoseStamped goal ++ ++# If the goal is obstructed, how many meters the planner can ++# relax the constraint in x and y before failing. ++float32 tolerance ++--- ++nav_msgs/Path plan +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv +new file mode 100644 +index 00000000..3b9caaad +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/LoadMap.srv +@@ -0,0 +1,15 @@ ++# URL of map resource ++# Can be an absolute path to a file: file:///path/to/maps/floor1.yaml ++# Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml ++string map_url ++--- ++# Result code defintions ++uint8 RESULT_SUCCESS=0 ++uint8 RESULT_MAP_DOES_NOT_EXIST=1 ++uint8 RESULT_INVALID_MAP_DATA=2 ++uint8 RESULT_INVALID_MAP_METADATA=3 ++uint8 RESULT_UNDEFINED_FAILURE=255 ++ ++# Returned map is only valid if result equals RESULT_SUCCESS ++nav_msgs/OccupancyGrid map ++uint8 result +diff --git a/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv b/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv +new file mode 100644 +index 00000000..55d3c24f +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/nav_msgs/srv/SetMap.srv +@@ -0,0 +1,6 @@ ++# Set a new map together with an initial pose ++nav_msgs/OccupancyGrid map ++geometry_msgs/PoseWithCovarianceStamped initial_pose ++--- ++bool success ++ +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt b/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt +index b2ae90cd..be86b1e7 100644 +--- a/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/CMakeLists.txt +@@ -18,6 +18,9 @@ add_message_files( + TwistStamped.msg + VehicleCmd.msg + InsStat.msg ++ PoseTwistStamped.msg ++ LaneWithPoseTwist.msg ++ LaneArrayWithPoseTwist.msg + DetectedObjectArray.msg + ) + +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg +index dae5bd5d..5c47b6ac 100644 +--- a/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/DetectedObjectArray.msg +@@ -1,2 +1,3 @@ + uint64 instance +-autoware_msgs/DetectedObjectArray object_array +\ No newline at end of file ++autoware_msgs/DetectedObjectArray object_array ++uint64 obj_instance +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg +new file mode 100644 +index 00000000..a7c1df6f +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneArrayWithPoseTwist.msg +@@ -0,0 +1,5 @@ ++uint64 instance ++autoware_msgs/LaneArray lane_array ++geometry_msgs/PoseStamped pose ++geometry_msgs/TwistStamped twist ++uint64 obj_instance +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg +new file mode 100644 +index 00000000..05da6d2a +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/LaneWithPoseTwist.msg +@@ -0,0 +1,5 @@ ++uint64 instance ++autoware_msgs/Lane lane ++geometry_msgs/PoseStamped pose ++geometry_msgs/TwistStamped twist ++uint64 obj_instance +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg +new file mode 100644 +index 00000000..03d0fa20 +--- /dev/null ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/PoseTwistStamped.msg +@@ -0,0 +1,3 @@ ++uint64 instance ++geometry_msgs/PoseStamped pose ++geometry_msgs/TwistStamped twist +\ No newline at end of file +diff --git a/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg b/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg +index b2cde46b..e4b90cd3 100644 +--- a/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg ++++ b/autoware.ai/src/autoware/messages/rubis_msgs/msg/TwistStamped.msg +@@ -1,2 +1,3 @@ + uint64 instance +-geometry_msgs/TwistStamped msg +\ No newline at end of file ++geometry_msgs/TwistStamped msg ++uint64 obj_instance +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch +new file mode 100644 +index 00000000..59c8f802 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch +@@ -0,0 +1,43 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch +new file mode 100644 +index 00000000..a14b621b +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch +@@ -0,0 +1,48 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch +new file mode 100644 +index 00000000..cd906d09 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch +new file mode 100644 +index 00000000..cf4d32cc +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch +@@ -0,0 +1,105 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch +new file mode 100644 +index 00000000..1fa79612 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch +@@ -0,0 +1,32 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..ee554f3a +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch +@@ -0,0 +1,44 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch +new file mode 100644 +index 00000000..59475396 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch +@@ -0,0 +1,51 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch +new file mode 100644 +index 00000000..66594cf2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch +new file mode 100644 +index 00000000..f25e94df +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch +@@ -0,0 +1,120 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch +new file mode 100644 +index 00000000..f66cef13 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch +@@ -0,0 +1,29 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/terminate.sh +new file mode 100644 +index 00000000..242e6f2c +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/terminate.sh +@@ -0,0 +1 @@ ++rosnode kill /cubetown +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch +new file mode 100644 +index 00000000..b410f94d +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch +@@ -0,0 +1,65 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch +new file mode 100644 +index 00000000..ab9971b3 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch +@@ -0,0 +1,60 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch +new file mode 100644 +index 00000000..6f2849ef +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch +@@ -0,0 +1,116 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch +new file mode 100644 +index 00000000..3653c630 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch +@@ -0,0 +1,32 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/terminate.sh +new file mode 100644 +index 00000000..230ba421 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/terminate.sh +@@ -0,0 +1 @@ ++rosnode kill /desktop_lane_keeping +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/test.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/test.launch +new file mode 100644 +index 00000000..1f08cd4a +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/test.launch +@@ -0,0 +1,26 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..a62287b4 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch +@@ -0,0 +1,58 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch +new file mode 100644 +index 00000000..49a153d4 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch +@@ -0,0 +1,57 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch +new file mode 100644 +index 00000000..66594cf2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch +new file mode 100644 +index 00000000..92401e07 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch +@@ -0,0 +1,105 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch +new file mode 100644 +index 00000000..41114441 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch +@@ -0,0 +1,38 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..be1dd9e2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch +@@ -0,0 +1,46 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch +new file mode 100644 +index 00000000..7d14bf71 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch +@@ -0,0 +1,20 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch +new file mode 100644 +index 00000000..66594cf2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch +new file mode 100644 +index 00000000..c15f91ce +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch +@@ -0,0 +1,106 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch +new file mode 100644 +index 00000000..7585a0a9 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch +@@ -0,0 +1,38 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..8d85ce76 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch +@@ -0,0 +1,57 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch +new file mode 100644 +index 00000000..af5aafc3 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch +@@ -0,0 +1,57 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch +new file mode 100644 +index 00000000..66594cf2 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch +@@ -0,0 +1,86 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch +new file mode 100644 +index 00000000..899f293d +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch +@@ -0,0 +1,106 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch +new file mode 100644 +index 00000000..1b3fd1e0 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch +@@ -0,0 +1,38 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch +new file mode 100644 +index 00000000..c788f638 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch +@@ -0,0 +1,35 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch +new file mode 100644 +index 00000000..cf4d32cc +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch +@@ -0,0 +1,105 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch +new file mode 100755 +index 00000000..38c45a8a +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch +@@ -0,0 +1,50 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch +new file mode 100755 +index 00000000..c9bb8c2b +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch +@@ -0,0 +1,96 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch +new file mode 100755 +index 00000000..06c2022c +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch +@@ -0,0 +1,16 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch +new file mode 100755 +index 00000000..b1c32eae +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch +@@ -0,0 +1,13 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch +new file mode 100755 +index 00000000..105bff0b +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch +@@ -0,0 +1,225 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch +new file mode 100755 +index 00000000..1bd2b8cf +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch +@@ -0,0 +1,40 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch +new file mode 100755 +index 00000000..5d64aed7 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch +@@ -0,0 +1,89 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch +new file mode 100755 +index 00000000..6d826bb0 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch +@@ -0,0 +1,13 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch +new file mode 100755 +index 00000000..e11fc7dd +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch +@@ -0,0 +1,30 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/terminate.sh +new file mode 100755 +index 00000000..b2c331c0 +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/terminate.sh +@@ -0,0 +1,56 @@ ++rosnode kill /lgsvl_triple_lidar_autorunner ++# rosnode kill /websocket_bridge ++# rosnode kill /lgsvl_traffic_signal_test ++# rosnode kill /image_republisher ++# rosnode kill /lidar_republisher_left ++# rosnode kill /lidar_republisher_right ++# rosnode kill /lidar_republisher_back ++# rosnode kill /camera_republisher ++# rosnode kill /gnss_republisher ++# rosnode kill /base_link_to_gnss ++# rosnode kill /base_link_to_velodyne ++# rosnode kill /vector_map_loader ++# rosnode kill /world_to_map ++# rosnode kill /map_to_mobility ++# rosnode kill /velodyne_to_velodyne_left ++# rosnode kill /velodyne_to_velodyne_right ++# rosnode kill /velodyne_to_velodyne_back ++# rosnode kill /pcd_clipper_left ++# rosnode kill /pcd_clipper_right ++# rosnode kill /pcd_clipper_back ++# rosnode kill /ray_ground_filter_left ++# rosnode kill /ray_ground_filter_right ++# rosnode kill /ray_ground_filter_back ++# rosnode kill /voxel_grid_filter ++# rosnode kill /gnss_localizer ++# rosnode kill /pose_relay ++# rosnode kill /vel_relay ++# rosnode kill /vision_darknet_detect ++# rosnode kill /yolo3_rects ++# rosnode kill /calibration_publisher_left ++# rosnode kill /calibration_publisher_right ++# rosnode kill /lidar_euclidean_cluster_detect_left ++# rosnode kill /lidar_euclidean_cluster_detect_right ++# rosnode kill /lidar_euclidean_cluster_detect_back ++# rosnode kill /imm_ukf_pda_left ++# rosnode kill /imm_ukf_pda_right ++# rosnode kill /imm_ukf_pda_back ++# rosnode kill /range_vision_fusion_left ++# rosnode kill /range_vision_fusion_right ++# rosnode kill /detection/fusion_tools/range_fusion_visualization_left ++# rosnode kill /detection/fusion_tools/range_fusion_visualization_right ++# rosnode kill /detection/lidar_detector/cluster_detect_visualization_back ++# rosnode kill /detection/lidar_detector/cluster_detect_visualization_left ++# rosnode kill /detection/lidar_detector/cluster_detect_visualization_right ++# rosnode kill /detection/object_tracker/ukf_track_visualization_back ++# rosnode kill /detection/object_tracker/ukf_track_visualization_left ++# rosnode kill /detection/object_tracker/ukf_track_visualization_right ++# rosnode kill op_global_planner ++# rosnode kill op_common_params ++# rosnode kill op_trajectory_generator ++# rosnode kill op_motion_predictor ++# rosnode kill predicted_objects_visualizer ++# rosnode kill op_trajectory_evaluator ++# rosnode kill op_behavior_selector ++# rosnode kill pure_pursuit ++# rosnode kill twist_filter +\ No newline at end of file +diff --git a/rubis_ws/deprecated/autorunner_scripts/transformation_test/_transformation_test_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/transformation_test/_transformation_test_1_sensing.launch +new file mode 100644 +index 00000000..807e075f +--- /dev/null ++++ b/rubis_ws/deprecated/autorunner_scripts/transformation_test/_transformation_test_1_sensing.launch +@@ -0,0 +1,54 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/scripts/carla_test.launch b/rubis_ws/scripts/carla_test.launch +index 24a95b77..deb37689 100755 +--- a/rubis_ws/scripts/carla_test.launch ++++ b/rubis_ws/scripts/carla_test.launch +@@ -11,7 +11,15 @@ + + + +- ++ ++ ++ ++ ++ ++ ++ ++ ++ + + + +diff --git a/rubis_ws/src/camera_image/src/camera_image.cpp b/rubis_ws/src/camera_image/src/camera_image.cpp +index 0c747228..90d26635 100644 +--- a/rubis_ws/src/camera_image/src/camera_image.cpp ++++ b/rubis_ws/src/camera_image/src/camera_image.cpp +@@ -11,8 +11,6 @@ static const std::string OPENCV_WINDOW = "Raw Image Window"; + + static int camera_id = 0; + static int frequency = 0; +-static int task_scheduling_flag = 0; +-static int task_profiling_flag = 0; + static std::string task_response_time_filename; + // static int rate = 0; // Frequency replaces rate + static double task_minimum_inter_release_time = 0; +@@ -65,15 +63,13 @@ int main(int argc, char** argv){ + + pnh.param("/camera_image/camera_id", camera_id, 0); + pnh.param("/camera_image/frequency", frequency, 10); +- pnh.param("/camera_image/task_scheduling_flag", task_scheduling_flag, 0); +- pnh.param("/camera_image/task_profiling_flag", task_profiling_flag, 0); + pnh.param("/camera_image/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/camera_image.csv"); + // pnh.param("/camera_image/rate", rate, 10); // Frequency replaces rate + pnh.param("/camera_image/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000); + pnh.param("/camera_image/task_execution_time", task_execution_time, (double)100000000); + pnh.param("/camera_image/task_relative_deadline", task_relative_deadline, (double)100000000); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + ROS_INFO("camera_id : %d / frequency : %d",camera_id, frequency); + if(!frequency){ +@@ -104,12 +100,7 @@ void CameraImage::sendImage(){ + + while(nh_.ok()){ + +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ rubis::start_task_profiling(); + + cap >> frame; + if(!frame.empty()){ +@@ -118,17 +109,12 @@ void CameraImage::sendImage(){ + msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); + msg->header.frame_id="camera"; + camera_image_pub_.publish(msg); +- rubis::sched::is_task_ready_ = TASK_STATE_DONE; + } + // int ckey = cv::waitKey(1); + // if(ckey == 27)break; + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); ++ rubis::stop_task_profiling(0, 0); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } + loop_rate.sleep(); + } + } +diff --git a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch +index d7e1860a..7ea93364 100755 +--- a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch ++++ b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch +@@ -28,14 +28,14 @@ + remap carla lidar to autoware. + @todo: to reduce load, Autoware should directly use the Carla-topic. + --> +- ++ + + +- ++ + + ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive b/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive +index 3685e92d..69b53989 100755 +--- a/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive ++++ b/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive +@@ -34,9 +34,10 @@ def callback(data): + """ + global wheelbase + msg = AckermannDrive() +- msg.speed = data.twist_cmd.twist.linear.x ++ msg.speed = data.ctrl_cmd.linear_velocity + msg.steering_angle = convert_trans_rot_vel_to_steering_angle( + msg.speed, data.twist_cmd.twist.angular.z, wheelbase) ++ msg.acceleration = data.ctrl_cmd.linear_acceleration + pub.publish(msg) + + def twist_to_ackermanndrive(): +diff --git a/rubis_ws/src/gnss_converter/CMakeLists.txt b/rubis_ws/src/gnss_converter/CMakeLists.txt +new file mode 100755 +index 00000000..8c7a4f57 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/CMakeLists.txt +@@ -0,0 +1,97 @@ ++cmake_minimum_required(VERSION 3.0.2) ++project(gnss_converter) ++ ++if(NOT CMAKE_BUILD_TYPE) ++ set(CMAKE_BUILD_TYPE Release) ++endif() ++ ++set(CMAKE_CXX_FLAGS_RELEASE "-Ofast") ++ ++## Compile as C++11, supported in ROS Kinetic and newer ++# add_compile_options(-std=c++11) ++ ++find_package(catkin REQUIRED COMPONENTS ++ roscpp ++ std_msgs ++ inertiallabs_msgs ++ geometry_msgs ++ message_filters ++ tf ++) ++ ++find_package(Eigen3 REQUIRED) ++set(EIGEN_PACKAGE EIGEN3) ++if(NOT EIGEN3_FOUND) ++ find_package(cmake_modules REQUIRED) ++ find_package(Eigen REQUIRED) ++ set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) ++ set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) ++ set(EIGEN_PACKAGE Eigen) ++endif() ++ ++find_package(OpenCV REQUIRED) ++ ++catkin_package( ++# INCLUDE_DIRS include ++# LIBRARIES gnss_converter ++# CATKIN_DEPENDS roscpp std_msgs ++# DEPENDS system_lib ++) ++ ++include_directories( ++ include ++ ${catkin_INCLUDE_DIRS} ++ ${OpenCV_INCLUDE_DIRS} ++) ++ ++ ++add_executable(gnss_converter ++ include/gnss_converter/gnss_converter.h ++ include/gnss_converter/LLH2UTM.h ++ include/gnss_converter/quaternion_euler.h ++ src/gnss_converter.cpp ++ src/LLH2UTM.cpp ++ src/quaternion_euler.cpp ++) ++add_dependencies(gnss_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ++target_link_libraries(gnss_converter ++ ${catkin_LIBRARIES} ++ ${EIGEN3_LIBRARIES} ++ ${OpenCV_LIBS} ++) ++ ++add_executable(gnss_pose_pub ++ include/gnss_converter/LLH2UTM.h ++ include/gnss_converter/quaternion_euler.h ++ src/gnss_pose_pub.cpp ++ src/LLH2UTM.cpp ++ src/quaternion_euler.cpp ++) ++add_dependencies(gnss_pose_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ++target_link_libraries(gnss_pose_pub ++ ${catkin_LIBRARIES} ++ ${EIGEN3_LIBRARIES} ++ ${OpenCV_LIBS} ++) ++ ++install( ++ TARGETS ++ gnss_converter ++ gnss_pose_pub ++ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ++ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ++ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ++) ++ ++install(DIRECTORY include/ ++ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} ++ PATTERN ".svn" EXCLUDE ++) ++install(DIRECTORY launch/ ++ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ++ PATTERN ".svn" EXCLUDE ++) ++ ++install(DIRECTORY cfg/ ++ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg ++) +diff --git a/rubis_ws/src/gnss_converter/README.md b/rubis_ws/src/gnss_converter/README.md +new file mode 100644 +index 00000000..f3594dcb +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/README.md +@@ -0,0 +1,43 @@ ++# How to use gnss_converter (/gnss_pose publisher) ++### Case 1 : If you create a new map ++* Record /Inertial_Labs/gps_data, /Inertial_Labs/ins_data, /ndt_pose, /ndt_stat data while driving through the map (rosbag) ++```console ++~$ rosbag record /ndt_pose /ndt_stat /Inertial_Labs/gps_data /Inertial_Labs/ins_data -O [NAME] ++``` ++* Set /gnss_converter/calculate_tf of gnss_converter/cfg/gnsss_converter.yaml to true. ++```yaml ++/gnss_converter/calculate_tf : true ++``` ++* Set /gnss_converter/bag_file_path of gnss_converter/cfg/gnsss_converter.yaml to recorded rosbag file. ++```yaml ++/gnss_converter/bag_file_path : /home/sunhokim/Documents/RUBIS/data/220111_pose_gnss_138ground.bag # example ++``` ++* Run the following code ++```console ++~$ roslaunch gnss_converter gnss_converter.launch ++``` ++* Wait about 40 seconds and a image showing the route of the vehicle will appear on the screen. ++* You can select the four points used to calculate the transformation matrix through the following method. ++ - To change the image size, use track bar to select a value and press enter. ++ - If you want to choose a point, press the number and click the point in the image. (RBUTTONDOWN) ++ - If you want to end, press the ESC button. ++ - **WARNING** : When performing angle transformation, linear transformation was assumed. However, since the angle has a value between -180 degrees and +180 degrees, if there is a discontinuous interval between the four points, the matrix is not properly calculated. Therefore, when selecting four points, it is necessary to make sure that there is no section where the change in angle is discontinuous. (If the first value of the ori_tf matrix is close to -1.0, the calculation was performed well.) ++* After the previous process, the transformation matrix calculation result is displayed on the screen. ++* The calculation result is transferred to gnss_converter/cfg/gnss_converter.yaml. ++```yaml ++# ========= position tf matrix ========= ++/gnss_converter/pos_tf : [-1.372640, -1.384772, -27.949656, 13944.211735, 0.410968, -0.109160, -11.105078, -9490.840625, -0.318483, -0.086243, -6.272676, 453.012563, 0.000000, -0.000000, 0.000000, 1.000000] ++ ++# ========= orientation tf matrix ========= ++/gnss_converter/ori_tf : [-1.000448, 0.345540, 0.569044, 0.987319, 0.021691, -2.279683, -4.681593, -0.067717, 0.056971, 1.608331, -3.444783, -0.114943, 0.000000, -0.000000, 0.000000, 1.000000] ++``` ++* Move to Case 2 ++### Case 2 : If you've already done Case 1 ++* Set /gnss_converter/calculate_tf of gnss_converter/cfg/gnsss_converter.yaml to false. ++```yaml ++/gnss_converter/calculate_tf : false ++``` ++* Execute gnss_converter launch file. ++```console ++~$ roslaunch gnss_converter gnss_converter.launch ++``` +diff --git a/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml b/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml +new file mode 100644 +index 00000000..fa359036 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml +@@ -0,0 +1,13 @@ ++# true : calculate tf matrix ++# false : publish /gnss_pose topic ++/gnss_converter/calculate_tf : true ++ ++/gnss_converter/bag_file_path : /home/sunhokim/Documents/RUBIS/data/220111_pose_gnss_138ground.bag ++ ++# map : 220111_138ground ++ ++# ========= position tf matrix ========= ++/gnss_converter/pos_tf : [-1.372640, -1.384772, -27.949656, 13944.211735, 0.410968, -0.109160, -11.105078, -9490.840625, -0.318483, -0.086243, -6.272676, 453.012563, 0.000000, -0.000000, 0.000000, 1.000000] ++ ++# ========= orientation tf matrix ========= ++/gnss_converter/ori_tf : [-1.000448, 0.345540, 0.569044, 0.987319, 0.021691, -2.279683, -4.681593, -0.067717, 0.056971, 1.608331, -3.444783, -0.114943, 0.000000, -0.000000, 0.000000, 1.000000] +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h b/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h +new file mode 100644 +index 00000000..68728bc5 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h +@@ -0,0 +1,27 @@ ++#include ++#include ++#include ++ ++#include ++ ++/*************** parameters for LLH to UTM transformation ***************/ ++ ++#define WGS84_A 6378137.0 // major axis ++#define WGS84_B 6356752.31424518 // minor axis ++#define WGS84_F 0.0033528107 // ellipsoid flattening ++#define WGS84_E 0.0818191908 // first eccentricity ++#define WGS84_EP 0.0820944379 // second eccentricity ++ ++// UTM Parameters ++#define UTM_K0 0.9996 // scale factor ++#define UTM_FE 500000.0 // false easting ++#define UTM_FN_N 0.0 // false northing, northern hemisphere ++#define UTM_FN_S 10000000.0 // false northing, southern hemisphere ++#define UTM_E2 (WGS84_E*WGS84_E) // e^2 ++#define UTM_E4 (UTM_E2*UTM_E2) // e^4 ++#define UTM_E6 (UTM_E4*UTM_E2) // e^6 ++#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2 ++ ++/*************************************************************************/ ++ ++void LLH2UTM(double Lat, double Long, double H, geometry_msgs::PoseStamped& pose); +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h b/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h +new file mode 100644 +index 00000000..410e74ad +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h +@@ -0,0 +1,70 @@ ++#include ++#include ++#include ++#include ++ ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++#include ++ ++#include ++ ++#include ++#include ++ ++#include ++#include ++#include ++#include ++ ++#include ++ ++#include ++#include ++#include ++#include ++ ++using namespace std; ++using namespace message_filters; ++using namespace Eigen; ++ ++#define ENTER_BUTTON 10 ++#define ESC_BUTTON 27 ++ ++struct gps_stat ++{ ++ std_msgs::Header header; ++ geometry_msgs::Point gps_pose; ++ geometry_msgs::Vector3 gps_ypr; ++ geometry_msgs::Point ndt_pose; ++ geometry_msgs::Vector3 ndt_ypr; ++ double ndt_score; ++}; ++ ++typedef sync_policies::ApproximateTime SyncPolicy_1; ++typedef sync_policies::ExactTime SyncPolicy_2; ++ ++static Matrix pos_tf_, ori_tf_; ++ ++static ros::Publisher gnss_pose_pub_; ++ ++static vector gps_backup_; ++static int ndt_pose_x_max_ = -9999999, ndt_pose_y_max_ = -9999999; ++static int ndt_pose_x_min_ = 9999999, ndt_pose_y_min_ = 9999999; ++static int scale_factor_ = 10; ++ ++static int points_idx_; ++static gps_stat selected_points_[4]; ++ ++void gps_ndt_data_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins, ++ const geometry_msgs::PoseStamped::ConstPtr &msg_ndt_pose); ++void pub_gnss_pose_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins); ++void track_bar_cb(int pos, void *userdata); ++void mouse_cb(int event, int x, int y, int flags, void *userdata); ++void points_select(); ++void calculate_tf_matrix(); +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h b/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h +new file mode 100644 +index 00000000..50b0c39d +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h +@@ -0,0 +1,7 @@ ++#include ++#include ++ ++#include ++ ++void ToEulerAngles(geometry_msgs::Quaternion q, double &yaw, double &pitch, double &roll); ++void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q); +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/launch/gnss_converter.launch b/rubis_ws/src/gnss_converter/launch/gnss_converter.launch +new file mode 100644 +index 00000000..658abfef +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/launch/gnss_converter.launch +@@ -0,0 +1,4 @@ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch b/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch +new file mode 100644 +index 00000000..8cdf1375 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch +@@ -0,0 +1,12 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/package.xml b/rubis_ws/src/gnss_converter/package.xml +new file mode 100644 +index 00000000..51cf2b79 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/package.xml +@@ -0,0 +1,23 @@ ++ ++ ++ gnss_converter ++ 0.0.0 ++ The gnss_converter package ++ ++ sunhokim ++ ++ Apache 2 ++ ++ catkin ++ roscpp ++ std_msgs ++ roscpp ++ std_msgs ++ roscpp ++ std_msgs ++ inertiallabs_msgs ++ message_filters ++ tf ++ eigen ++ libopencv-dev ++ +diff --git a/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp b/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp +new file mode 100644 +index 00000000..a39d4698 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp +@@ -0,0 +1,54 @@ ++#include ++ ++void LLH2UTM(double Lat, double Long, double H, geometry_msgs::PoseStamped& pose){ ++ double a = WGS84_A; ++ double eccSquared = UTM_E2; ++ double k0 = UTM_K0; ++ double LongOrigin; ++ double eccPrimeSquared; ++ double N, T, C, A, M; ++ // Make sure the longitude is between -180.00 .. 179.9 ++ // (JOQ: this is broken for Long < -180, do a real normalize) ++ double LongTemp = (Long+180)-int((Long+180)/360)*360-180; ++ double LatRad = angles::from_degrees(Lat); ++ double LongRad = angles::from_degrees(LongTemp); ++ double LongOriginRad; ++ pose.pose.position.z = H; ++ // Fix Zone number with Korea ++ int zone = 52; ++ char band = 'S'; ++ // +3 puts origin in middle of zone ++ LongOrigin = (zone - 1)*6 - 180 + 3; ++ LongOriginRad = angles::from_degrees(LongOrigin); ++ eccPrimeSquared = (eccSquared)/(1-eccSquared); ++ N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); ++ T = tan(LatRad)*tan(LatRad); ++ C = eccPrimeSquared*cos(LatRad)*cos(LatRad); ++ A = cos(LatRad)*(LongRad-LongOriginRad); ++ M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 ++ - 5*eccSquared*eccSquared*eccSquared/256) * LatRad ++ - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 ++ + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) ++ + (15*eccSquared*eccSquared/256 ++ + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) ++ - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); ++ pose.pose.position.y = (double) ++ (k0*N*(A+(1-T+C)*A*A*A/6 ++ + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) ++ + 500000.0); ++ pose.pose.position.x = (double) ++ (k0*(M+N*tan(LatRad) ++ *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 ++ + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); ++ ++ double TM[4][4] = ++ {{-0.821456, -0.593423, -0.006448, 3606301.475406}, ++ {-0.596954, 0.803991, -0.096993, 2231713.639404}, ++ {0.049875, 0.018177, -0.047063, -213252.081285}, ++ {0.000000, 0.000000, 0.000000, 1.000000}}; ++ ++ double input[4] = {pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, 1}; ++ pose.pose.position.x = TM[0][0]*input[0] + TM[0][1]*input[1] + TM[0][2]*input[2] + TM[0][3]*input[3]; ++ pose.pose.position.y = TM[1][0]*input[0] + TM[1][1]*input[1] + TM[1][2]*input[2] + TM[1][3]*input[3]; ++ pose.pose.position.z = TM[2][0]*input[0] + TM[2][1]*input[1] + TM[2][2]*input[2] + TM[2][3]*input[3]; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/src/gnss_converter.cpp b/rubis_ws/src/gnss_converter/src/gnss_converter.cpp +new file mode 100644 +index 00000000..964e49b6 +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/src/gnss_converter.cpp +@@ -0,0 +1,363 @@ ++#include ++#include ++#include ++ ++void gps_ndt_data_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins, ++ const geometry_msgs::PoseStamped::ConstPtr &msg_ndt_pose) ++{ ++ ++ geometry_msgs::PoseStamped cur_pose; ++ ++ double ndt_yaw, ndt_pitch, ndt_roll; ++ ++ LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, cur_pose); ++ ++ ToEulerAngles(msg_ndt_pose->pose.orientation, ndt_yaw, ndt_pitch, ndt_roll); ++ ++ gps_stat tmp; ++ geometry_msgs::Vector3 vec_tmp; ++ ++ tmp.header = msg_gps->header; ++ ++ // gps position & orientation data ++ tmp.gps_pose = cur_pose.pose.position; ++ vec_tmp.x = ((msg_ins->YPR.x > 180) ? (msg_ins->YPR.x - 360) : (msg_ins->YPR.x)) / 180 * M_PI; ++ vec_tmp.y = ((msg_ins->YPR.y > 180) ? (msg_ins->YPR.y - 360) : (msg_ins->YPR.y)) / 180 * M_PI; ++ vec_tmp.z = ((msg_ins->YPR.z > 180) ? (msg_ins->YPR.z - 360) : (msg_ins->YPR.z)) / 180 * M_PI; ++ tmp.gps_ypr = vec_tmp; ++ ++ // ndt position & orientation data ++ tmp.ndt_pose = msg_ndt_pose->pose.position; ++ vec_tmp.x = ndt_yaw; ++ vec_tmp.y = ndt_pitch; ++ vec_tmp.z = ndt_roll; ++ tmp.ndt_ypr = vec_tmp; ++ ++ if (ndt_pose_x_max_ < msg_ndt_pose->pose.position.x) ++ ndt_pose_x_max_ = msg_ndt_pose->pose.position.x; ++ ++ if (ndt_pose_x_min_ > msg_ndt_pose->pose.position.x) ++ ndt_pose_x_min_ = msg_ndt_pose->pose.position.x; ++ ++ if (ndt_pose_y_max_ < msg_ndt_pose->pose.position.y) ++ ndt_pose_y_max_ = msg_ndt_pose->pose.position.y; ++ ++ if (ndt_pose_y_min_ > msg_ndt_pose->pose.position.y) ++ ndt_pose_y_min_ = msg_ndt_pose->pose.position.y; ++ ++ gps_backup_.push_back(tmp); ++} ++ ++void pub_gnss_pose_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins) ++{ ++ geometry_msgs::PoseStamped cur_pose; ++ ++ cur_pose.header = msg_ins->header; ++ cur_pose.header.frame_id = "/map"; ++ ++ Matrix gps; ++ Matrix ndt; ++ ++ /*================= position calculation =================*/ ++ LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, cur_pose); ++ ++ gps(0, 0) = cur_pose.pose.position.x; ++ gps(1, 0) = cur_pose.pose.position.y; ++ gps(2, 0) = cur_pose.pose.position.z; ++ gps(3, 0) = 1.0; ++ ++ ndt = pos_tf_ * gps; ++ ++ cur_pose.pose.position.x = ndt(0, 0); ++ cur_pose.pose.position.y = ndt(1, 0); ++ cur_pose.pose.position.z = ndt(2, 0); ++ /*=======================================================*/ ++ ++ /*=============== orientation calculation ===============*/ ++ gps(0, 0) = ((msg_ins->YPR.x > 180) ? (msg_ins->YPR.x - 360) : (msg_ins->YPR.x)) / 180 * M_PI; ++ gps(1, 0) = ((msg_ins->YPR.y > 180) ? (msg_ins->YPR.y - 360) : (msg_ins->YPR.y)) / 180 * M_PI; ++ gps(2, 0) = ((msg_ins->YPR.z > 180) ? (msg_ins->YPR.z - 360) : (msg_ins->YPR.z)) / 180 * M_PI; ++ gps(3, 0) = 1.0; ++ ++ ndt = ori_tf_ * gps; ++ ++ ToQuaternion(ndt(0, 0), ndt(1, 0), ndt(2, 0), cur_pose.pose.orientation); ++ ++ gnss_pose_pub_.publish(cur_pose); ++ /*=======================================================*/ ++} ++ ++void scale_image(int pos, void *userdata) ++{ ++ scale_factor_ = ((pos < 1) ? (1) : (pos)); ++} ++ ++void mouse_cb(int event, int x, int y, int flags, void *userdata) ++{ ++ if (event == cv::EVENT_RBUTTONDOWN) ++ { ++ double point_x = ndt_pose_x_min_ + x / scale_factor_; ++ double point_y = ndt_pose_y_min_ + y / scale_factor_; ++ double min_dist = 9999; ++ int idx; ++ ++ for (int i = 0; i < gps_backup_.size(); i++) ++ { ++ if (pow((point_x - gps_backup_[i].ndt_pose.x), 2) + pow((point_y - gps_backup_[i].ndt_pose.y), 2) < pow(min_dist, 2)) ++ { ++ idx = i; ++ min_dist = pow((point_x - gps_backup_[i].ndt_pose.x), 2) + pow((point_y - gps_backup_[i].ndt_pose.y), 2); ++ } ++ } ++ ++ selected_points_[points_idx_] = gps_backup_[idx]; ++ std::cout << "**************** Point INFO ****************" << std::endl; ++ ++ std::cout << "GPS Pose" << std::endl; ++ std::cout << " x : " << selected_points_[points_idx_].gps_pose.x << std::endl; ++ std::cout << " y : " << selected_points_[points_idx_].gps_pose.y << std::endl; ++ std::cout << " z : " << selected_points_[points_idx_].gps_pose.z << std::endl; ++ ++ std::cout << "GPS YPR" << std::endl; ++ std::cout << " yaw : " << selected_points_[points_idx_].gps_ypr.x / M_PI * 180 << std::endl; ++ std::cout << " pitch : " << selected_points_[points_idx_].gps_ypr.y / M_PI * 180 << std::endl; ++ std::cout << " roll : " << selected_points_[points_idx_].gps_ypr.z / M_PI * 180 << std::endl; ++ ++ std::cout << "NDT Pose" << std::endl; ++ std::cout << " x : " << selected_points_[points_idx_].ndt_pose.x << std::endl; ++ std::cout << " y : " << selected_points_[points_idx_].ndt_pose.y << std::endl; ++ std::cout << " z : " << selected_points_[points_idx_].ndt_pose.z << std::endl; ++ ++ std::cout << "NDT YPR" << std::endl; ++ std::cout << " yaw : " << selected_points_[points_idx_].ndt_ypr.x / M_PI * 180 << std::endl; ++ std::cout << " pitch : " << selected_points_[points_idx_].ndt_ypr.y / M_PI * 180 << std::endl; ++ std::cout << " roll : " << selected_points_[points_idx_].ndt_ypr.z / M_PI * 180 << std::endl; ++ ++ std::cout << "NDT score" << selected_points_[points_idx_].ndt_score << std::endl; ++ } ++} ++ ++void points_select() ++{ ++ cv::Mat orig_img = cv::Mat((ndt_pose_y_max_ - ndt_pose_y_min_ + 1), (ndt_pose_x_max_ - ndt_pose_x_min_ + 1), CV_8UC3, cv::Scalar(255, 255, 255)); ++ cv::Mat display_img; ++ ++ for (int i = 0; i < gps_backup_.size(); i++) ++ { ++ cv::circle(orig_img, cv::Point((gps_backup_[i].ndt_pose.x - ndt_pose_x_min_), (gps_backup_[i].ndt_pose.y - ndt_pose_y_min_)), ++ 1, cv::Scalar(255, 255, 0), 1, cv::LINE_AA); ++ } ++ ++ orig_img.copyTo(display_img); ++ ++ cv::namedWindow("Display Image"); ++ cv::createTrackbar("Scale", "Display Image", &scale_factor_, 100, scale_image, NULL); ++ ++ cv::setMouseCallback("Display Image", mouse_cb, NULL); ++ ++ std::cout << "**************** How To Use ****************" << std::endl; ++ std::cout << "If you want to end, press the ESC button." << std::endl; ++ std::cout << "If you want to choose a point, press the number and click the point in the image." << std::endl; ++ std::cout << "To change the image size, use track bar to select a value and press enter." << std::endl; ++ std::cout << "********************************************" << std::endl; ++ ++ int keycode; ++ while (true) ++ { ++ display_img = cv::Mat(orig_img.rows * scale_factor_, orig_img.cols * scale_factor_, CV_8UC3, cv::Scalar(255, 255, 255)); ++ for (int i = 0; i < gps_backup_.size(); i++) ++ { ++ cv::circle(display_img, ++ cv::Point(scale_factor_ * (gps_backup_[i].ndt_pose.x - ndt_pose_x_min_), scale_factor_ * (gps_backup_[i].ndt_pose.y - ndt_pose_y_min_)), ++ 1, cv::Scalar(255, 255, 0), 1, cv::LINE_AA); ++ } ++ ++ cv::imshow("Display Image", display_img); ++ keycode = cv::waitKey(); ++ ++ if (keycode == ESC_BUTTON) ++ break; ++ ++ switch (keycode) ++ { ++ case '1': ++ points_idx_ = 0; ++ std::cout << "Point Number : 1" << std::endl; ++ break; ++ case '2': ++ points_idx_ = 1; ++ std::cout << "Point Number : 2" << std::endl; ++ break; ++ case '3': ++ points_idx_ = 2; ++ std::cout << "Point Number : 3" << std::endl; ++ break; ++ case '4': ++ points_idx_ = 3; ++ std::cout << "Point Number : 4" << std::endl; ++ break; ++ case ENTER_BUTTON: ++ break; ++ default: ++ std::cout << "Unknown keyboard input" << std::endl; ++ break; ++ } ++ } ++} ++ ++void calculate_tf_matrix() ++{ ++ Matrix gps_pos, ndt_pos, gps_ypr, ndt_ypr; ++ ++ for (int i = 0; i < 4; i++) ++ { ++ gps_pos(0, i) = selected_points_[i].gps_pose.x; ++ gps_pos(1, i) = selected_points_[i].gps_pose.y; ++ gps_pos(2, i) = selected_points_[i].gps_pose.z; ++ gps_pos(3, i) = 1.0; ++ ++ ndt_pos(0, i) = selected_points_[i].ndt_pose.x; ++ ndt_pos(1, i) = selected_points_[i].ndt_pose.y; ++ ndt_pos(2, i) = selected_points_[i].ndt_pose.z; ++ ndt_pos(3, i) = 1.0; ++ ++ gps_ypr(0, i) = selected_points_[i].gps_ypr.x; ++ gps_ypr(1, i) = selected_points_[i].gps_ypr.y; ++ gps_ypr(2, i) = selected_points_[i].gps_ypr.z; ++ gps_ypr(3, i) = 1.0; ++ ++ ndt_ypr(0, i) = selected_points_[i].ndt_ypr.x; ++ ndt_ypr(1, i) = selected_points_[i].ndt_ypr.y; ++ ndt_ypr(2, i) = selected_points_[i].ndt_ypr.z; ++ ndt_ypr(3, i) = 1.0; ++ } ++ ++ pos_tf_ = ndt_pos * gps_pos.inverse(); ++ ori_tf_ = ndt_ypr * gps_ypr.inverse(); ++ ++ printf("========== position tf matrix ==========\n"); ++ printf("[%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf]\n", pos_tf_(0, 0), pos_tf_(0, 1), pos_tf_(0, 2), pos_tf_(0, 3), pos_tf_(1, 0), pos_tf_(1, 1), pos_tf_(1, 2), pos_tf_(1, 3), pos_tf_(2, 0), pos_tf_(2, 1), pos_tf_(2, 2), pos_tf_(2, 3), pos_tf_(3, 0), pos_tf_(3, 1), pos_tf_(3, 2), pos_tf_(3, 3)); ++ printf("======== orientation tf matrix =========\n"); ++ printf("[%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf]\n", ori_tf_(0, 0), ori_tf_(0, 1), ori_tf_(0, 2), ori_tf_(0, 3), ori_tf_(1, 0), ori_tf_(1, 1), ori_tf_(1, 2), ori_tf_(1, 3), ori_tf_(2, 0), ori_tf_(2, 1), ori_tf_(2, 2), ori_tf_(2, 3), ori_tf_(3, 0), ori_tf_(3, 1), ori_tf_(3, 2), ori_tf_(3, 3)); ++} ++ ++int main(int argc, char *argv[]) ++{ ++ ros::init(argc, argv, "gnss_converter"); ++ ++ ros::NodeHandle nh; ++ ++ bool calculate_tf; ++ ++ gnss_pose_pub_ = nh.advertise("/gnss_pose", 10); ++ ++ message_filters::Subscriber gps_sub(nh, "/Inertial_Labs/gps_data", 10); ++ message_filters::Subscriber ndt_pose_sub(nh, "/ndt_pose", 10); ++ message_filters::Subscriber ins_sub(nh, "/Inertial_Labs/ins_data", 10); ++ ++ Synchronizer sync_1(SyncPolicy_1(100), gps_sub, ins_sub, ndt_pose_sub); ++ Synchronizer sync_2(SyncPolicy_2(10), gps_sub, ins_sub); ++ ++ ros::param::get("/gnss_converter/calculate_tf", calculate_tf); ++ ++ if (calculate_tf) ++ { ++ sync_1.registerCallback(boost::bind(&gps_ndt_data_cb, _1, _2, _3)); ++ ++ pid_t pid; ++ if ((pid = fork()) < 0) ++ ROS_ERROR("Cannot create child!"); ++ ++ if (pid == 0) ++ { ++ std::string file_path; ++ char file_path_cstr[150]; ++ ++ ros::param::get("/gnss_converter/bag_file_path", file_path); ++ ++ int str_len = file_path.length(); ++ if (str_len >= 150) ++ { ++ ROS_ERROR("File path is too long!!"); ++ exit(0); ++ } ++ ++ strcpy(file_path_cstr, file_path.c_str()); ++ ++ const char *file = "/opt/ros/melodic/bin/rosbag"; ++ char *exe_argv[] = {"/opt/ros/melodic/bin/rosbag", ++ "play", ++ "-r", ++ "5", ++ file_path_cstr, ++ NULL}; ++ ++ if (execvp(file, exe_argv) < 0) ++ { ++ ROS_ERROR("Cannot load bag file!!"); ++ } ++ } ++ else ++ { ++ int wstatus; ++ while (waitpid(pid, &wstatus, WNOHANG) == 0) ++ { ++ ros::spinOnce(); ++ } ++ ROS_WARN("Finish loading data from rosbag file!!"); ++ points_select(); ++ calculate_tf_matrix(); ++ } ++ } ++ ++ else ++ { ++ sync_2.registerCallback(boost::bind(&pub_gnss_pose_cb, _1, _2)); ++ ++ vector tf_tmp; ++ ++ /*================= pos_tf_ matrix =================*/ ++ ros::param::get("/gnss_converter/pos_tf", tf_tmp); ++ pos_tf_(0, 0) = tf_tmp[0]; ++ pos_tf_(0, 1) = tf_tmp[1]; ++ pos_tf_(0, 2) = tf_tmp[2]; ++ pos_tf_(0, 3) = tf_tmp[3]; ++ pos_tf_(1, 0) = tf_tmp[4]; ++ pos_tf_(1, 1) = tf_tmp[5]; ++ pos_tf_(1, 2) = tf_tmp[6]; ++ pos_tf_(1, 3) = tf_tmp[7]; ++ pos_tf_(2, 0) = tf_tmp[8]; ++ pos_tf_(2, 1) = tf_tmp[9]; ++ pos_tf_(2, 2) = tf_tmp[10]; ++ pos_tf_(2, 3) = tf_tmp[11]; ++ pos_tf_(3, 0) = tf_tmp[12]; ++ pos_tf_(3, 1) = tf_tmp[13]; ++ pos_tf_(3, 2) = tf_tmp[14]; ++ pos_tf_(3, 3) = tf_tmp[15]; ++ /*=================================================*/ ++ ++ /*================= ori_tf_ matrix =================*/ ++ ros::param::get("/gnss_converter/ori_tf", tf_tmp); ++ ori_tf_(0, 0) = tf_tmp[0]; ++ ori_tf_(0, 1) = tf_tmp[1]; ++ ori_tf_(0, 2) = tf_tmp[2]; ++ ori_tf_(0, 3) = tf_tmp[3]; ++ ori_tf_(1, 0) = tf_tmp[4]; ++ ori_tf_(1, 1) = tf_tmp[5]; ++ ori_tf_(1, 2) = tf_tmp[6]; ++ ori_tf_(1, 3) = tf_tmp[7]; ++ ori_tf_(2, 0) = tf_tmp[8]; ++ ori_tf_(2, 1) = tf_tmp[9]; ++ ori_tf_(2, 2) = tf_tmp[10]; ++ ori_tf_(2, 3) = tf_tmp[11]; ++ ori_tf_(3, 0) = tf_tmp[12]; ++ ori_tf_(3, 1) = tf_tmp[13]; ++ ori_tf_(3, 2) = tf_tmp[14]; ++ ori_tf_(3, 3) = tf_tmp[15]; ++ /*================================================*/ ++ ++ ros::spin(); ++ } ++ ++ return 0; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp b/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp +new file mode 100644 +index 00000000..ad248edd +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp +@@ -0,0 +1,91 @@ ++#include ++#include ++ ++#include ++#include ++#include ++ ++#include ++#include ++#include ++#include ++ ++#include ++#include ++#include ++#include ++ ++#define M_PI 3.14159265358979323846 ++ ++using namespace std; ++using namespace message_filters; ++ ++typedef sync_policies::ExactTime SyncPolicy_; ++ ++static ros::Publisher gnss_pose_pub_; ++static double x_offset_, y_offset_, yaw_offset_; ++static double roll_, pitch_, yaw_; ++static geometry_msgs::PoseStamped gnss_pose_; ++ ++void gnss_pose_pub_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins){ ++ gnss_pose_.header = msg_gps->header; ++ gnss_pose_.header.frame_id = "/map"; ++ ++ LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, gnss_pose_); ++ ++ gnss_pose_.pose.position.x = gnss_pose_.pose.position.x - x_offset_; ++ gnss_pose_.pose.position.y = gnss_pose_.pose.position.y - y_offset_; ++ ++ roll_ = msg_ins->YPR.z; ++ pitch_ = msg_ins->YPR.y; ++ ++ yaw_ = msg_ins->YPR.x; ++ yaw_ *= -1; ++ yaw_ -= yaw_offset_; ++ if(yaw_ > 180.0) yaw_ -= 360.0; ++ if(yaw_ < -180.0) yaw_ += 360.0; ++ ++ roll_ = roll_ * M_PI/180.0; ++ pitch_ = pitch_ * M_PI/180.0; ++ yaw_ = yaw_ * M_PI/180.0; ++} ++ ++int main(int argc, char *argv[]){ ++ ros::init(argc, argv, "gnss_pose_pub"); ++ ++ ros::NodeHandle nh; ++ ++ nh.param("/gnss_pose_pub/x_offset", x_offset_, 0.0); ++ nh.param("/gnss_pose_pub/y_offset", y_offset_, 0.0); ++ nh.param("/ins_twist_generator/yaw_offset", yaw_offset_, 0.0); ++ ++ gnss_pose_pub_ = nh.advertise("/ndt_pose", 2); ++ ++ message_filters::Subscriber gps_sub(nh, "/Inertial_Labs/gps_data", 2); ++ message_filters::Subscriber ins_sub(nh, "/Inertial_Labs/ins_data", 2); ++ ++ Synchronizer sync(SyncPolicy_(2), gps_sub, ins_sub); ++ ++ sync.registerCallback(boost::bind(&gnss_pose_pub_cb, _1, _2)); ++ ++ tf::TransformBroadcaster br; ++ ros::Rate r(10); ++ while(nh.ok()){ ++ ros::spinOnce(); ++ ++ ToQuaternion(yaw_, pitch_, roll_, gnss_pose_.pose.orientation); ++ ++ tf::Quaternion q; ++ q.setRPY(roll_, pitch_, yaw_); ++ ++ tf::StampedTransform transform; ++ transform.setOrigin(tf::Vector3(gnss_pose_.pose.position.x, gnss_pose_.pose.position.y, 0.0)); ++ transform.setRotation(q); ++ br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/map", "/base_link")); ++ ++ gnss_pose_pub_.publish(gnss_pose_); ++ ++ ++ r.sleep(); ++ } ++} +\ No newline at end of file +diff --git a/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp b/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp +new file mode 100644 +index 00000000..158c979b +--- /dev/null ++++ b/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp +@@ -0,0 +1,32 @@ ++#include ++ ++void ToEulerAngles(geometry_msgs::Quaternion q, double &yaw, double &pitch, double &roll){ ++ double sinr_cosp = 2 * (q.w * q.x + q.y * q.z); ++ double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y); ++ roll = std::atan2(sinr_cosp, cosr_cosp); ++ ++ double sinp = 2 * (q.w * q.y - q.z * q.x); ++ if (std::abs(sinp) >= 1) ++ pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range ++ else ++ pitch = std::asin(sinp); ++ ++ double siny_cosp = 2 * (q.w * q.z + q.x * q.y); ++ double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); ++ yaw = std::atan2(siny_cosp, cosy_cosp); ++} ++ ++void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q) ++{ ++ double cy = cos(yaw * 0.5); ++ double sy = sin(yaw * 0.5); ++ double cp = cos(pitch * 0.5); ++ double sp = sin(pitch * 0.5); ++ double cr = cos(roll * 0.5); ++ double sr = sin(roll * 0.5); ++ ++ q.w = cr * cp * cy + sr * sp * sy; ++ q.x = sr * cp * cy - cr * sp * sy; ++ q.y = cr * sp * cy + sr * cp * sy; ++ q.z = cr * cp * sy - sr * sp * cy; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/image_common/image_transport/src/republish.cpp b/rubis_ws/src/image_common/image_transport/src/republish.cpp +index 589e351a..2a8140d6 100644 +--- a/rubis_ws/src/image_common/image_transport/src/republish.cpp ++++ b/rubis_ws/src/image_common/image_transport/src/republish.cpp +@@ -54,23 +54,19 @@ int main(int argc, char** argv) + + // scheduling + ros::NodeHandle pnh("~"); +- int task_scheduling_flag = 0; +- int task_profiling_flag = 0; + std::string task_response_time_filename; + int rate = 0; + double task_minimum_inter_release_time = 0; + double task_execution_time = 0; + double task_relative_deadline = 0; + +- pnh.param("/republish/task_scheduling_flag", task_scheduling_flag, 0); +- pnh.param("/republish/task_profiling_flag", task_profiling_flag, 0); + pnh.param("/republish/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/republish.csv"); + pnh.param("/republish/rate", rate, 10); + pnh.param("/republish/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000); + pnh.param("/republish/task_execution_time", task_execution_time, (double)100000000); + pnh.param("/republish/task_relative_deadline", task_relative_deadline, (double)100000000); + +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + + if (argc < 3) { + // Use all available transports for output +@@ -81,34 +77,17 @@ int main(int argc, char** argv) + PublishMemFn pub_mem_fn = &image_transport::Publisher::publish; + sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, &pub, _1), ros::VoidPtr(), in_transport); + +- rubis::sched::task_state_ = TASK_STATE_READY; ++ ros::Rate r(rate); ++ // Executing task ++ while(ros::ok()){ ++ rubis::start_task_profiling(); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ ros::spinOnce(); ++ + +- ros::spinOnce(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ rubis::stop_task_profiling(0, 0); + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ r.sleep(); + } + } + else { +@@ -128,32 +107,14 @@ int main(int argc, char** argv) + PublishMemFn pub_mem_fn = &Plugin::publish; + sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, pub.get(), _1), pub, in_transport); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- rubis::sched::task_state_ = TASK_STATE_DONE; +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(0, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ ros::Rate r(rate); ++ // Executing task ++ while(ros::ok()){ ++ rubis::start_task_profiling(); ++ ros::spinOnce(); ++ ++ rubis::stop_task_profiling(0, 0); ++ r.sleep(); + } + } + +diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt b/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt +index 66aa1a13..bf1bf2a9 100644 +--- a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt ++++ b/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/CMakeLists.txt +@@ -15,7 +15,6 @@ inertiallabs_msgs + + include_directories(${catkin_INCLUDE_DIRS}) + +-find_package(OpenCV) + + set(DEPRECATION_FLAG "-Wsizeof-array-argument") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra ${DEPRECATION_FLAG}") +@@ -26,7 +25,6 @@ catkin_package( + DEPENDS system_lib + ) + +-include_directories(${Opencv_INCLUDE_DIRS}) + include_directories(${system_lib_INCLUDE_DIRS}) + + include_directories(../inertiallabs_sdk/) +diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml +index c49e3f72..40654dad 100755 +--- a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml ++++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml +@@ -15,4 +15,4 @@ carla_ackermann_control: + # at more or less constant speed. + # If the absolute value of the ackermann drive target acceleration exceeds this value, + # directly the input acceleration is controlled +- min_accel: 1.0 ++ min_accel: 0.005 +diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup +new file mode 100755 +index 00000000..c49e3f72 +--- /dev/null ++++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup +@@ -0,0 +1,18 @@ ++carla_ackermann_control: ++ ros__parameters: ++ # override the default values of the pid speed controller ++ # (only relevant for ackermann control mode) ++ speed_Kp: 0.05 # min: 0, max: 1. ++ speed_Ki: 0.00 # min: 0, max: 1. ++ speed_Kd: 0.50 # min: 0, max: 10. ++ # override the default values of the pid acceleration controller ++ # (only relevant for ackermann control mode) ++ accel_Kp: 0.05 # min: 0, max: 10. ++ accel_Ki: 0.00 # min: 0, max: 10. ++ accel_Kd: 0.05 # min: 0, max: 10. ++ # set the minimum acceleration in (m/s^2) ++ # This border value is used to enable the speed controller which is used to control driving ++ # at more or less constant speed. ++ # If the absolute value of the ackermann drive target acceleration exceeds this value, ++ # directly the input acceleration is controlled ++ min_accel: 1.0 +diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py b/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py +index 944f70fd..c8d29c00 100755 +--- a/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py ++++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py +@@ -442,6 +442,7 @@ class CarlaAckermannControl(CompatibleNode): + else: + if self.info.status.speed_control_activation_count > 0: + self.info.status.speed_control_activation_count -= 1 ++ + # set the auto_mode of the controller accordingly + self.speed_controller.auto_mode = self.info.status.speed_control_activation_count >= 5 + +diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json +index 9aef8118..d31f2c9f 100755 +--- a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json ++++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json +@@ -55,13 +55,14 @@ + "type": "sensor.lidar.ray_cast", + "id": "lidar", + "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, +- "range": 50, +- "channels": 32, ++ "range": 100, ++ "channels": 16, + "points_per_second": 320000, + "upper_fov": 2.0, + "lower_fov": -26.8, + "rotation_frequency": 20, +- "noise_stddev": 0.0 ++ "noise_stddev": 0.0, ++ "sensor_tick": 0.1 + }, + { + "type": "sensor.other.gnss", +diff --git a/rubis_ws/src/rubis_autorunner/CMakeLists.txt b/rubis_ws/src/rubis_autorunner/CMakeLists.txt +index 4932f0b1..be91e751 100644 +--- a/rubis_ws/src/rubis_autorunner/CMakeLists.txt ++++ b/rubis_ws/src/rubis_autorunner/CMakeLists.txt +@@ -48,6 +48,18 @@ add_executable(minicar_lane_keeping + src/minicar_lane_keeping/minicar_lane_keeping.cpp + ) + ++add_executable(carla_lkas_autorunner ++ include/carla_autorunner/carla_autorunner.h ++ src/carla_autorunner/carla_lkas_autorunner_node.cpp ++ src/carla_autorunner/carla_lkas_autorunner.cpp ++) ++ ++add_executable(carla_full_autorunner ++ include/carla_autorunner/carla_autorunner.h ++ src/carla_autorunner/carla_full_autorunner_node.cpp ++ src/carla_autorunner/carla_full_autorunner.cpp ++) ++ + add_executable(cubetown_lkas_autorunner + include/cubetown_autorunner/cubetown_autorunner.h + src/cubetown_autorunner/cubetown_lkas_autorunner_node.cpp +@@ -102,6 +114,19 @@ target_link_libraries(cubetown_full_autorunner + ros_autorunner_lib + ) + ++add_dependencies(carla_lkas_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ++target_link_libraries(carla_lkas_autorunner ++ ${catkin_LIBRARIES} ++ ros_autorunner_lib ++) ++ ++add_dependencies(carla_full_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ++target_link_libraries(carla_full_autorunner ++ ${catkin_LIBRARIES} ++ ros_autorunner_lib ++) ++ ++ + add_dependencies(tutorial_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(tutorial_autorunner + ${catkin_LIBRARIES} +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml +new file mode 100644 +index 00000000..feb87474 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml +@@ -0,0 +1,183 @@ ++# Unit of rate: hz ++# Unit of scheduling params: ns ++ ++# 1s : 1_000_000_000 ++# 1ms : 1_000_000 ++# 1us : 1_000 ++# 1ns : 1 ++ ++#rate = hz ++# other time unut = ns ++ ++# Lane Keeping ++lidar_republisher: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 99 ++ exec_time: 1_000_000 ++ deadline: 2_000_000 ++ period: 2_000_000 ++ task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" ++ ++voxel_grid_filter: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 99 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" ++ ++ndt_matching: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" ++ ++ use_kalman_filter: False ++ tf_x: 1.0510799 ++ tf_y: 0 ++ tf_z: 1.96 ++ tf_roll: 0 ++ tf_pitch: 0 ++ tf_yaw: 0 ++ localizer: "velodyne" ++ ++pure_pursuit: ++ rate: 30 #30 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" ++ ++ dynamic_params_flag: False ++ dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" ++ ++twist_filter: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" ++ ++# Detection ++ray_ground_filter_center: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" ++ ++lidar_euclidean_cluster_detect: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" ++ ++ network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" ++ pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" ++ ++# Planning ++op_global_planner: ++ rate: 25 #25 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" ++ ++ multilap_flag: 1 ++ ++op_common_params: ++ rollOutDensity: 4 ++ rollOutsNumber: 2 ++ maxVelocity: 10.0 ++ maxAcceleration: 10.0 ++ maxDeceleration: -10.0 ++ ++op_trajectory_generator: ++ rate: 100 #100 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" ++ ++op_trajectory_evaluator: ++ rate: 100 #100 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" ++ ++ weightPriority: 1 ++ weightTransition: 0.5 ++ weightLong: 1 ++ weightLat: 1 ++ ImageWidth: 1920 ++ ImageHeight: 1080 ++ SprintDecisionTime: 9999999.0 ++ ++op_behavior_selector: ++ rate: 100 #100 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" ++ ++ distanceToPedestrianThreshold: 15.0 ++ sprintSpeed: 10.0 ++ obstacleWaitingTimeinIntersection: 2.0 ++ turnThreshold: 30.0 ++ ++op_motion_predictor: ++ rate: 25 #25 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" ++ ++# Independent ++twist_gate: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" ++ ++ zero_flag: 0 ## Publish target velocity as 0 +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml +new file mode 100644 +index 00000000..83c242dd +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml +@@ -0,0 +1,14 @@ ++total_step_num: 5 ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh" ++# step_# : ++# [0] pacakge name ++# [1] target name( node name or launch script name ) ++# [2] state that create constraint topic or not ( "true" or "false" ) ++# [3] state that check constraint topic or not ( "true" or "false" ) ++# [4] target type( RUN or LAUNCH ) ++ ++step_1: ["rubis_autorunner", "_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"] ++step_2: ["rubis_autorunner", "_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"] ++step_3: ["rubis_autorunner", "_carla_autorunner_3_detection.launch", "true", "true", "LAUNCH"] ++step_4: ["rubis_autorunner", "_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"] ++step_5: ["rubis_autorunner", "_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"] +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml +new file mode 100644 +index 00000000..6fed9fc7 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml +@@ -0,0 +1,13 @@ ++total_step_num: 4 ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh" ++# step_# : ++# [0] pacakge name ++# [1] target name( node name or launch script name ) ++# [2] state that create constraint topic or not ( "true" or "false" ) ++# [3] state that check constraint topic or not ( "true" or "false" ) ++# [4] target type( RUN or LAUNCH ) ++ ++step_1: ["rubis_autorunner", "_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"] ++step_2: ["rubis_autorunner", "_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"] ++step_3: ["rubis_autorunner", "_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"] ++step_4: ["rubis_autorunner", "_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"] +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla/lane_info.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/lane_info.yaml +similarity index 100% +rename from rubis_ws/src/rubis_autorunner/cfg/carla/lane_info.yaml +rename to rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/lane_info.yaml +diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml +similarity index 80% +rename from rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml +rename to rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml +index 1680abe6..a7f3c4f3 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/params.yaml +@@ -12,7 +12,7 @@ + # Sensing + ray_ground_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -22,7 +22,7 @@ ray_ground_filter: + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -33,17 +33,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 100 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + logger_brake_dist: + rate: 10 +@@ -55,37 +50,27 @@ calibration_publisher: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 100 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -94,7 +79,7 @@ imm_ukf_pda_track: + + range_vision_fusion: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -104,7 +89,7 @@ range_vision_fusion: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -122,7 +107,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -131,7 +116,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100000000 +@@ -154,7 +139,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -167,7 +152,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 40000000 +@@ -177,7 +162,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 33333333 +@@ -188,7 +173,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -197,7 +182,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml.backup b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml.backup +index 4e759a90..40581461 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml.backup ++++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner.yaml.backup +@@ -1,5 +1,5 @@ + total_step_num: 5 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml +index dacb51ae..a9545721 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml +@@ -1,30 +1,46 @@ + # Unit of rate: hz +-# Unit of task information: nano seconds ++# Unit of scheduling params: ns + +-# 1s : 1000000000 +-# 1ms : 1000000 +-# 1us : 1000 ++# 1s : 1_000_000_000 ++# 1ms : 1_000_000 ++# 1us : 1_000 + # 1ns : 1 + + #rate = hz + # other time unut = ns + +-# Localization +-voxel_grid_filter: +- instance_mode: 1 ++# Lane Keeping ++lidar_republisher: ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 99 ++ exec_time: 1_000_000 ++ deadline: 2_000_000 ++ period: 2_000_000 ++ task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" ++voxel_grid_filter: + rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 1_600_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 99 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + + ndt_matching: +- instance_mode: 1 +- use_kalman_filter: True +- ++ rate: 10 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" ++ ++ use_kalman_filter: False + tf_x: 1.0510799 + tf_y: 0 + tf_z: 1.96 +@@ -33,129 +49,93 @@ ndt_matching: + tf_yaw: 0 + localizer: "velodyne" + +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 80_000_000 +- task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" +- +-modular_ndt_matching_CENTER: +- instance_mode: 1 +- use_kalman_filter: True ++pure_pursuit: ++ rate: 30 #30 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + +- tf_x: 1.0510799 +- tf_y: 0 +- tf_z: 1.96 +- tf_roll: 0 +- tf_pitch: 0 +- tf_yaw: 0 +- localizer: "velodyne" ++ dynamic_params_flag: False ++ dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" ++twist_filter: + rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 80_000_000 +- task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + + # Detection + ray_ground_filter_center: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 100_000_000 +- task_relative_deadline: 100_000_000 +- ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + + lidar_euclidean_cluster_detect: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 +- task_minimum_inter_release_time: 200_000_000 +- task_execution_time: 50_000_000 +- task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" +- +-vision_darknet_detect: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 75_000_000 +- task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" ++ + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + +-imm_ukf_pda_track: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" +- rate: 10 +- task_minimum_inter_release_time: 200_000_000 +- task_execution_time: 100_000_000 +- task_relative_deadline: 200_000_000 +- + # Planning + op_global_planner: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 45_000_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" ++ + multilap_flag: 1 + + op_common_params: +- rollOutDensity: 2 +- rollOutsNumber: 4 ++ rollOutDensity: 4 ++ rollOutsNumber: 2 + maxVelocity: 10.0 + maxAcceleration: 10.0 + maxDeceleration: -10.0 + + op_trajectory_generator: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 2_900_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + + op_trajectory_evaluator: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 3_600_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" ++ + weightPriority: 1 + weightTransition: 0.5 + weightLong: 10 +@@ -165,77 +145,39 @@ op_trajectory_evaluator: + SprintDecisionTime: 9999999.0 + + op_behavior_selector: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 20_800_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" ++ + distanceToPedestrianThreshold: 15.0 + sprintSpeed: 10.0 + obstacleWaitingTimeinIntersection: 2.0 + turnThreshold: 30.0 + + op_motion_predictor: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 3_800_000 +- task_relative_deadline: 100_000_000 +- +-# Control +-pure_pursuit: +- instance_mode: 1 +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" +- rate: 30 #30 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 3_000_000 +- task_relative_deadline: 100_000_000 +- dynamic_params_flag: False +- dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" +- +-twist_filter: +- instance_mode: 1 +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 2_000_000 +- task_relative_deadline: 100_000_000 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + ++# Independent + twist_gate: +- instance_mode: 1 +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 2_000_000 +- task_relative_deadline: 100_000_000 +- zero_flag: 0 ## Publish target velocity as 0 ++ task_scheduling_configs: ++ policy: "NONE" ++ priority: 20 ++ exec_time: 0 # ns ++ deadline: 0 # ns ++ period: 0 # ns ++ task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + +-# Others +-lidar_republisher: +- instance_mode: 1 +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 70_000_000 +- task_relative_deadline: 100_000_000 +- +-republish: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" +- rate: 10 +- task_minimum_inter_release_time: 100_000_000 +- task_execution_time: 35_000_000 +- task_relative_deadline: 100_000_000 +\ No newline at end of file ++ zero_flag: 0 ## Publish target velocity as 0 +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup +deleted file mode 100644 +index f8e2f9cf..00000000 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_autorunner_params.yaml.backup ++++ /dev/null +@@ -1,238 +0,0 @@ +-# Unit of rate: hz +-# Unit of task information: nano seconds +- +-# 1s : 1000000000 +-# 1ms : 1000000 +-# 1us : 1000 +-# 1ns : 1 +- +-#rate = hz +-# other time unut = ns +- +-# Localization +-voxel_grid_filter: +- task_scheduling_flag: 1 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-ndt_matching: +- tf_x: 1.0510799 +- tf_y: 0 +- tf_z: 1.96 +- tf_roll: 0 +- tf_pitch: 0 +- tf_yaw: 0 +- localizer: "velodyne" +- +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 70000000 +- task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" +- +-# Detection +-compare_map_filter: +- distance_threshold: 0.4 +- min_clipping_height: -1.7 +- max_clipping_height: 0.5 +- +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 70000000 +- task_relative_deadline: 100000000 +- +- +-lidar_euclidean_cluster_detect: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" +- +-vision_darknet_detect: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" +- network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg" +- pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights" +- +-imm_ukf_pda_track: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-range_vision_fusion: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-# Planning +-op_global_planner: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" +- rate: 25 #25 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- multilap_flag: 1 +- +-op_common_params: +- rollOutDensity: 4 +- rollOutsNumber: 0 +- maxVelocity: 10.0 +- maxAcceleration: 10.0 +- maxDeceleration: -10.0 +- +-op_trajectory_generator: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" +- rate: 100 #100 +- task_minimum_inter_release_time: 10000000 +- task_execution_time: 10000000 +- task_relative_deadline: 10000000 +- +-op_trajectory_evaluator: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" +- rate: 100 #100 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- weightPriority: 0 +- weightTransition: 5 +- weightLong: 4 +- weightLat: 4 +- ImageWidth: 1920 +- ImageHeight: 1080 +- SprintDecisionTime: 9999999.0 +- +-op_behavior_selector: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" +- rate: 100 #100 +- task_minimum_inter_release_time: 10000000 +- task_execution_time: 10000000 +- task_relative_deadline: 10000000 +- distanceToPedestrianThreshold: 15.0 +- sprintSpeed: 10.0 +- obstacleWaitingTimeinIntersection: 2.0 +- turnThreshold: 30.0 +- +-op_motion_predictor: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" +- rate: 25 #25 +- task_minimum_inter_release_time: 40000000 +- task_execution_time: 40000000 +- task_relative_deadline: 40000000 +- +-# Control +-pure_pursuit: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" +- rate: 30 #30 +- task_minimum_inter_release_time: 33333333 +- task_execution_time: 33333333 +- task_relative_deadline: 33333333 +- dynamic_params_flag: False +- dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" +- +-twist_filter: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-twist_gate: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- zero_flag: 0 ## Publish target velocity as 0 +- +-# Others +-lidar_republisher: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-vel_relay: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-vel_relay: +- task_scheduling_flag: 0 +- task_profiling_flag: 0 +- task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +- +-republish: +- task_scheduling_flag: 0 +- task_profiling_flag: 1 +- task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" +- rate: 10 +- task_minimum_inter_release_time: 100000000 +- task_execution_time: 100000000 +- task_relative_deadline: 100000000 +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml +index 4e759a90..40581461 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_full_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 5 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml +index c70a8e39..6873e2f3 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/cubetown_autorunner/cubetown_lkas_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 4 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml +index 3548041c..f142af8a 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lane_keeping.yaml +@@ -1,5 +1,5 @@ + total_step_num: 4 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml +index 29cc8960..0817c790 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/desktop_lane_keeping/desktop_lene_keeping_params.yaml +@@ -12,7 +12,7 @@ + # Sensing + ray_ground_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -22,7 +22,7 @@ ray_ground_filter: + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -33,17 +33,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + calibration_publisher: +@@ -51,37 +46,27 @@ calibration_publisher: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 100000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -90,7 +75,7 @@ imm_ukf_pda_track: + + range_vision_fusion: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -100,7 +85,7 @@ range_vision_fusion: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -118,7 +103,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -127,7 +112,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100000000 +@@ -143,7 +128,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -156,7 +141,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 40000000 +@@ -166,7 +151,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 33333333 +@@ -177,7 +162,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -186,7 +171,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml +index 446ed3b2..0eaf97db 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params.yaml +@@ -19,7 +19,7 @@ voxel_grid_filter: + instance_mode: 1 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -30,20 +30,14 @@ modular_ndt_matching_FR: + instance_mode: 1 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + ndt_matching: +- instance_mode: 1 + use_kalman_filter: False + + tf_x: 3.3 +@@ -55,24 +49,17 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" +- +- + + # Detection + ray_ground_filter_center: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -82,37 +69,27 @@ ray_ground_filter_center: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 + task_execution_time: 50_000_000 + task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 75_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 +@@ -122,7 +99,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 10 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -139,7 +116,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 10 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -148,7 +125,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 10 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -166,7 +143,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 10 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -179,7 +156,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 10 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -188,9 +165,8 @@ op_motion_predictor: + + # Control + pure_pursuit: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 10 #30 + task_minimum_inter_release_time: 100_000_000 +@@ -200,9 +176,8 @@ pure_pursuit: + dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + + twist_filter: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -210,9 +185,8 @@ twist_filter: + task_relative_deadline: 100_000_000 + + twist_gate: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -222,9 +196,8 @@ twist_gate: + + # Others + lidar_republisher: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -233,7 +206,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -241,9 +214,8 @@ vel_relay: + task_relative_deadline: 100_000_000 + + pose_relay: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -252,7 +224,7 @@ pose_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml +index cb127f78..f979d738 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/ionic_autorunner/ionic_autorunner_params_spin40.yaml +@@ -18,7 +18,7 @@ voxel_grid_filter: + instance_mode: 1 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -29,20 +29,14 @@ modular_ndt_matching_FR: + instance_mode: 1 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + ndt_matching: +- instance_mode: 1 + use_kalman_filter: False + + tf_x: 3.3 +@@ -54,24 +48,17 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" +- +- + + # Detection + ray_ground_filter_center: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -81,37 +68,27 @@ ray_ground_filter_center: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 40 + task_minimum_inter_release_time: 200_000_000 + task_execution_time: 50_000_000 + task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 75_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 40 + task_minimum_inter_release_time: 200_000_000 +@@ -121,7 +98,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 40 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -138,7 +115,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 40 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -147,7 +124,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 40 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -165,7 +142,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 40 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -178,7 +155,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 40 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -187,9 +164,8 @@ op_motion_predictor: + + # Control + pure_pursuit: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 40 #30 + task_minimum_inter_release_time: 100_000_000 +@@ -199,9 +175,8 @@ pure_pursuit: + dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + + twist_filter: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -209,9 +184,8 @@ twist_filter: + task_relative_deadline: 100_000_000 + + twist_gate: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -221,9 +195,8 @@ twist_gate: + + # Others + lidar_republisher: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -232,7 +205,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -240,9 +213,8 @@ vel_relay: + task_relative_deadline: 100_000_000 + + pose_relay: +- instance_mode: 1 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +@@ -251,7 +223,7 @@ pose_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 40 + task_minimum_inter_release_time: 100_000_000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml +index 717968c5..e0e4d6ab 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 9 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml +index 1f7f7e4e..b1249827 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lane_keeping.yaml +@@ -1,5 +1,5 @@ + total_step_num: 4 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/minicar_lane_keeping/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/minicar_lane_keeping/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml +index 17751521..b2066de6 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/minicar_lane_keeping/minicar_lene_keeping_params.yaml +@@ -15,7 +15,7 @@ camera_image: + frequency: 10 + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + # rate: 10 # Frequency replaces rate + task_minimum_inter_release_time: 100000000 +@@ -24,7 +24,7 @@ camera_image: + + ray_ground_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -34,7 +34,7 @@ ray_ground_filter: + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -45,22 +45,17 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 + task_execution_time: 70000000 + task_relative_deadline: 100000000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100000000 +@@ -78,7 +73,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -87,7 +82,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100000000 +@@ -103,7 +98,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 10000000 +@@ -117,7 +112,7 @@ op_behavior_selector: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 33333333 +@@ -128,7 +123,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -137,7 +132,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -148,7 +143,7 @@ twist_gate: + # Others + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +@@ -157,7 +152,7 @@ vel_relay: + + pose_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100000000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml +index 7c9a8b8d..b4235314 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 5 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml +index 59cfa745..ccd8802d 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/rubis_testbed_autorunner/rubis_testbed_autorunner_params.yaml +@@ -14,7 +14,7 @@ voxel_grid_filter: + instance_mode: 0 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -33,22 +33,17 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + ray_ground_filter_center: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -58,37 +53,27 @@ ray_ground_filter_center: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 + task_execution_time: 50_000_000 + task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 75_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 0 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-320.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 +@@ -98,7 +83,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -115,7 +100,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -124,7 +109,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -140,7 +125,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -153,7 +138,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -162,9 +147,8 @@ op_motion_predictor: + + # Control + pure_pursuit: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 100_000_000 +@@ -174,9 +158,8 @@ pure_pursuit: + dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + + twist_filter: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -184,9 +167,8 @@ twist_filter: + task_relative_deadline: 100_000_000 + + twist_gate: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -196,9 +178,8 @@ twist_gate: + + # Others + lidar_republisher: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -207,7 +188,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -215,9 +196,8 @@ vel_relay: + task_relative_deadline: 100_000_000 + + pose_relay: +- instance_mode: 0 + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -226,7 +206,7 @@ pose_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +diff --git a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml +index 7bbaa3d4..89161057 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autorunner.yaml +@@ -1,5 +1,5 @@ + total_step_num: 5 +-terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/tutorial/terminate.sh" ++terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/tutorial/terminate_desktop.sh" + # step_# : + # [0] pacakge name + # [1] target name( node name or launch script name ) +diff --git a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml +index 5ff124da..39f6cf02 100644 +--- a/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml ++++ b/rubis_ws/src/rubis_autorunner/cfg/tutorial_autorunner/tutorial_autoruuner_params.yaml +@@ -12,7 +12,7 @@ + # Localization + voxel_grid_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 0 ++ + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -29,17 +29,12 @@ ndt_matching: + localizer: "velodyne" + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 80_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 1 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" + + # Detection + compare_map_filter: +@@ -48,7 +43,7 @@ compare_map_filter: + max_clipping_height: 0.5 + + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/compare_map_filter.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 +@@ -58,37 +53,27 @@ compare_map_filter: + + lidar_euclidean_cluster_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 + task_execution_time: 50_000_000 + task_relative_deadline: 200_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 1 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/clustering_gpu_deadline.csv" + + vision_darknet_detect: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 + task_execution_time: 75_000_000 + task_relative_deadline: 80_000_000 +- gpu_scheduling_flag: 0 +- gpu_profiling_flag: 1 +- gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" +- gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" +- gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3-tiny.weights" + + imm_ukf_pda_track: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" + rate: 10 + task_minimum_inter_release_time: 200_000_000 +@@ -98,7 +83,7 @@ imm_ukf_pda_track: + # Planning + op_global_planner: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -115,7 +100,7 @@ op_common_params: + + op_trajectory_generator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -124,7 +109,7 @@ op_trajectory_generator: + + op_trajectory_evaluator: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -140,7 +125,7 @@ op_trajectory_evaluator: + + op_behavior_selector: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + rate: 100 #100 + task_minimum_inter_release_time: 100_000_000 +@@ -153,7 +138,7 @@ op_behavior_selector: + + op_motion_predictor: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + rate: 25 #25 + task_minimum_inter_release_time: 100_000_000 +@@ -163,7 +148,7 @@ op_motion_predictor: + # Control + pure_pursuit: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + rate: 30 #30 + task_minimum_inter_release_time: 100_000_000 +@@ -174,7 +159,7 @@ pure_pursuit: + + twist_filter: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -183,7 +168,7 @@ twist_filter: + + twist_gate: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -194,7 +179,7 @@ twist_gate: + # Others + lidar_republisher: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -203,7 +188,7 @@ lidar_republisher: + + vel_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/vel_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -212,7 +197,7 @@ vel_relay: + + pose_relay: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/pose_relay.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +@@ -221,7 +206,7 @@ pose_relay: + + republish: + task_scheduling_flag: 0 +- task_profiling_flag: 1 ++ + task_response_time_filename: "~/Documents/profiling/response_time/republish.csv" + rate: 10 + task_minimum_inter_release_time: 100_000_000 +diff --git a/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h b/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h +new file mode 100644 +index 00000000..66bf4692 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/include/carla_autorunner/carla_autorunner.h +@@ -0,0 +1,32 @@ ++#include ++#include ++ ++// Include subscribe message type ++#include ++#include ++#include ++#include ++#include ++#include ++#define SLEEP_PERIOD 1 ++ ++class CarlaAutorunner : public AutorunnerBase{ ++private: ++ ros::NodeHandle nh_; ++ ROSAutorunner ros_autorunner_; ++private: ++ virtual void register_subscribers(); ++private: ++ void points_raw_cb(const sensor_msgs::PointCloud2& msg); ++ void ndt_pose_cb(const geometry_msgs::PoseStamped& msg); ++ void detection_cb(const autoware_msgs::DetectedObjectArray& msg); ++ void behavior_state_cb(const visualization_msgs::MarkerArray& msg); ++ ++public: ++ Sub_v sub_v_; ++ ros::Publisher initial_pose_pub_; ++public: ++ CarlaAutorunner() {} ++ CarlaAutorunner(ros::NodeHandle nh) : nh_(nh){} ++ virtual void Run(); ++}; +diff --git a/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h b/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h +index a7a7086a..84f62d3a 100644 +--- a/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h ++++ b/rubis_ws/src/rubis_autorunner/include/cubetown_autorunner/cubetown_autorunner.h +@@ -6,6 +6,7 @@ + #include + #include + #include ++#include + #include + #define SLEEP_PERIOD 1 + +@@ -17,7 +18,7 @@ private: + virtual void register_subscribers(); + private: + void points_raw_cb(const sensor_msgs::PointCloud2& msg); +- void ndt_stat_cb(const autoware_msgs::NDTStat& msg); ++ void ndt_pose_cb(const geometry_msgs::PoseStamped& msg); + void detection_cb(const autoware_msgs::DetectedObjectArray& msg); + void behavior_state_cb(const visualization_msgs::MarkerArray& msg); + +diff --git a/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch +new file mode 100644 +index 00000000..a91009bc +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/launch/carla_full_autorunner.launch +@@ -0,0 +1,4 @@ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch b/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch +new file mode 100644 +index 00000000..41f53b5f +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/launch/carla_lkas_autorunner.launch +@@ -0,0 +1,4 @@ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch +new file mode 100644 +index 00000000..50ce0be4 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_1_sensing.launch +@@ -0,0 +1,46 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch +new file mode 100644 +index 00000000..9f6dca50 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch +@@ -0,0 +1,54 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch +new file mode 100644 +index 00000000..df533eb6 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch +@@ -0,0 +1,42 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch +new file mode 100644 +index 00000000..8bb60870 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch +@@ -0,0 +1,64 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch +new file mode 100644 +index 00000000..4f5d25e6 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch +@@ -0,0 +1,118 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch +new file mode 100644 +index 00000000..59144340 +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch +@@ -0,0 +1,32 @@ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch +index 072b973a..fdf35425 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch +@@ -22,17 +22,21 @@ + + + +- ++ + + + + + + +- + +- ++ + + +- ++ ++ ++ ++ ++ + ++ +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch +index cf62f8fb..8afcb8a1 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch +@@ -36,11 +36,11 @@ + pitch: $(arg init_pitch), + yaw: $(arg init_yaw), + use_predict_pose: 1, +- error_threshold: 0.01, +- resolution: 3.0, +- step_size: 0.3, ++ error_threshold: 0.05, ++ resolution: 1.0, ++ step_size: 0.5, + trans_epsilon: 0.01, +- max_iterations: 10} ++ max_iterations: 2} + '"/> + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch +index a49a410e..819a2793 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch +@@ -82,11 +82,18 @@ + + + ++ ++ + + + + + ++ ++ ++ ++ ++ + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch +index 68b2046e..6fbbc3a2 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch +@@ -15,7 +15,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch +index a8f5c037..b399ee97 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch +@@ -21,24 +21,19 @@ + + + +- +- + + + +- + + + + + +- + + + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch +index fd7ce414..05ed4386 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch +@@ -15,7 +15,6 @@ + + + +- + + + +@@ -33,7 +32,6 @@ + + + +- + + + +@@ -78,7 +76,6 @@ + + + +- + + + +@@ -123,7 +120,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch +index cab08d89..546fc8d7 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch +@@ -36,22 +36,18 @@ + + + +- + + + + + + +- +- +- ++ + + + + + +- + + + +@@ -76,7 +72,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch +index e78be3d2..44a69ccc 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch +@@ -15,7 +15,6 @@ + + + +- + + + +@@ -23,7 +22,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch +index 2a317ad2..c71066ba 100644 +--- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch ++++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch +@@ -14,7 +14,6 @@ + + + +- + + + +@@ -32,7 +31,6 @@ + + + +- + + + +diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp +new file mode 100644 +index 00000000..e5f44ead +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp +@@ -0,0 +1,108 @@ ++#include "carla_autorunner/carla_autorunner.h" ++ ++void CarlaAutorunner::Run(){ ++ register_subscribers(); // Register subscribers that shoud check can go next or not ++ ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner ++ ros::Rate rate(1); // Rate can be changed ++ while(ros::ok()){ ++ if(!ros_autorunner_.Run()) break; // Run Autorunner ++ ros::spinOnce(); ++ rate.sleep(); ++ } ++} ++ ++void CarlaAutorunner::register_subscribers(){ ++ int total_step_num = nh_.param("/total_step_num", -1); ++ if(total_step_num < 0){ ++ std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); ++} ++ ++ void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ ++ if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ ++ ROS_WARN("[STEP 1] Map and Sensors are prepared"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; ++ } ++ } ++ ++ void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ ++ static int failure_cnt = 0, success_cnt = 0; ++ failure_cnt++; ++ ++ static const double pos_x = 314.072479248; ++ static const double pos_y = 129.654495239; ++ static const double pos_z = 0.044597864151; ++ ++ static const double ori_x = 0.0; ++ static const double ori_y = 0.0; ++ static const double ori_z = 0.70715665039; ++ static const double ori_w = 0.70705690407; ++ ++ ++ if(failure_cnt > 10){ ++ std::cout<<"# Refresh inital pose"<= pos_x - 1.5 && ++ msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 && ++ msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 && ++ msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 && ++ msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 && ++ msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 && ++ !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ ++ success_cnt++; ++ if(success_cnt < 3) return; ++ ROS_WARN("[STEP 2] Localization is success"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; ++ } ++ else{ ++ success_cnt = 0; ++ } ++ } ++ ++void CarlaAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){ ++ if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){ ++ ROS_WARN("[STEP 3] All detection modules are excuted"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; ++ } ++} ++ ++ ++ void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ ++ std::string state = msg.markers.front().text; ++ if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ ++ ROS_WARN("[STEP 4] Global & local planning success"); ++ ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true; ++ } ++} ++ ++ ++ +diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp +new file mode 100644 +index 00000000..caac332f +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp +@@ -0,0 +1,11 @@ ++#include ++ ++int main(int argc, char* argv[]){ ++ ros::init(argc, argv, "carla_autorunner"); ++ ros::NodeHandle nh; ++ ++ CarlaAutorunner carla_autorunner(nh); ++ carla_autorunner.Run(); ++ ++ return 0; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp +new file mode 100644 +index 00000000..df8f65dd +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp +@@ -0,0 +1,99 @@ ++#include "carla_autorunner/carla_autorunner.h" ++ ++void CarlaAutorunner::Run(){ ++ register_subscribers(); // Register subscribers that shoud check can go next or not ++ ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner ++ ros::Rate rate(1); // Rate can be changed ++ while(ros::ok()){ ++ if(!ros_autorunner_.Run()) break; // Run Autorunner ++ ros::spinOnce(); ++ rate.sleep(); ++ } ++} ++ ++void CarlaAutorunner::register_subscribers(){ ++ int total_step_num = nh_.param("/total_step_num", -1); ++ if(total_step_num < 0){ ++ std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); ++} ++ ++ void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ ++ if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ ++ ROS_WARN("[STEP 1] Map and Sensors are prepared"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; ++ } ++ } ++ ++ void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ ++ static int failure_cnt = 0, success_cnt = 0; ++ failure_cnt++; ++ ++ static const double pos_x = 314.072479248; ++ static const double pos_y = 129.654495239; ++ static const double pos_z = 0.044597864151; ++ ++ static const double ori_x = 0.0; ++ static const double ori_y = 0.0; ++ static const double ori_z = 0.70715665039; ++ static const double ori_w = 0.70705690407; ++ ++ ++ if(failure_cnt > 10){ ++ std::cout<<"# Refresh inital pose"<= pos_x - 1.5 && ++ msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 && ++ msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 && ++ msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 && ++ msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 && ++ msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 && ++ !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ ++ success_cnt++; ++ if(success_cnt < 3) return; ++ ROS_WARN("[STEP 2] Localization is success"); ++ sleep(SLEEP_PERIOD); ++ ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; ++ } ++ else{ ++ success_cnt = 0; ++ } ++ } ++ ++ ++ void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ ++ std::string state = msg.markers.front().text; ++ if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ ++ ROS_WARN("[STEP 3] Global & local planning success"); ++ ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; ++ } ++} ++ ++ ++ +diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp +new file mode 100644 +index 00000000..caac332f +--- /dev/null ++++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp +@@ -0,0 +1,11 @@ ++#include ++ ++int main(int argc, char* argv[]){ ++ ros::init(argc, argv, "carla_autorunner"); ++ ros::NodeHandle nh; ++ ++ CarlaAutorunner carla_autorunner(nh); ++ carla_autorunner.Run(); ++ ++ return 0; ++} +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp +index 62a816ed..2dc84b89 100644 +--- a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp ++++ b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp +@@ -21,7 +21,7 @@ void CubetownAutorunner::register_subscribers(){ + + // Set the check function(subscriber) + sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CubetownAutorunner::points_raw_cb, this); +- sub_v_[STEP(2)] = nh_.subscribe("/ndt_stat", 1, &CubetownAutorunner::ndt_stat_cb, this); ++ sub_v_[STEP(2)] = nh_.subscribe("/ndt_pose", 1, &CubetownAutorunner::ndt_pose_cb, this); + sub_v_[STEP(3)] = nh_.subscribe("/detection/lidar_detector/objects_center", 1, &CubetownAutorunner::detection_cb, this); + sub_v_[STEP(4)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this); + +@@ -36,27 +36,37 @@ void CubetownAutorunner::register_subscribers(){ + } + } + +- void CubetownAutorunner::ndt_stat_cb(const autoware_msgs::NDTStat& msg){ +- static int cnt = 0; +- cnt++; +- if(cnt > 200){ +- geometry_msgs::PoseWithCovariance msg; +- msg.pose.position.x = 56.3796081543; +- msg.pose.position.y = -0.0106279850006; +- msg.pose.position.z = 0.465716004372; +- msg.pose.orientation.x = -0.00171861096474; +- msg.pose.orientation.y = -0.00120572400155; +- msg.pose.orientation.z = 0.707457658123; +- msg.pose.orientation.w = 0.706752612; +- initial_pose_pub_.publish(msg); +- cnt = 0; ++ void CubetownAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ ++ static int failure_cnt = 0, success_cnt = 0; ++ failure_cnt++; ++ ++ if(failure_cnt > 10){ ++ std::cout<<"# Refresh inital pose"<= 55.0 && ++ msg.pose.position.y >= -0.20 && msg.pose.position.y <= 0.00 && ++ !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ ++ success_cnt++; ++ if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } ++ else{ ++ success_cnt = 0; ++ } + + } + +diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp +index fecbc2c3..422d1062 100644 +--- a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp ++++ b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp +@@ -21,7 +21,7 @@ void CubetownAutorunner::register_subscribers(){ + + // Set the check function(subscriber) + sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CubetownAutorunner::points_raw_cb, this); +- sub_v_[STEP(2)] = nh_.subscribe("/ndt_stat", 1, &CubetownAutorunner::ndt_stat_cb, this); ++ sub_v_[STEP(2)] = nh_.subscribe("/ndt_pose", 1, &CubetownAutorunner::ndt_pose_cb, this); + sub_v_[STEP(3)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this); + + initial_pose_pub_ = nh_.advertise< geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1); +@@ -35,29 +35,41 @@ void CubetownAutorunner::register_subscribers(){ + } + } + +- void CubetownAutorunner::ndt_stat_cb(const autoware_msgs::NDTStat& msg){ +- static int cnt = 0; +- cnt++; +- if(cnt > 200){ +- geometry_msgs::PoseWithCovariance msg; +- msg.pose.position.x = 56.3796081543; +- msg.pose.position.y = -0.0106279850006; +- msg.pose.position.z = 0.465716004372; +- msg.pose.orientation.x = -0.00171861096474; +- msg.pose.orientation.y = -0.00120572400155; +- msg.pose.orientation.z = 0.707457658123; +- msg.pose.orientation.w = 0.706752612; +- initial_pose_pub_.publish(msg); +- cnt = 0; ++ void CubetownAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ ++ static int failure_cnt = 0, success_cnt = 0; ++ failure_cnt++; ++ ++ if(failure_cnt > 10){ ++ std::cout<<"# Refresh inital pose"<= 55.0 && ++ msg.pose.position.y >= -0.20 && msg.pose.position.y <= 0.00 && ++ !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ ++ success_cnt++; ++ if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } ++ else{ ++ success_cnt = 0; ++ } ++ + } + ++ + void CubetownAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ + std::string state = msg.markers.front().text; + if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ +diff --git a/rubis_ws/src/rubis_pkg/include/gnss_localizer.h b/rubis_ws/src/rubis_pkg/include/gnss_localizer.h +index 317a5987..1edf9048 100644 +--- a/rubis_ws/src/rubis_pkg/include/gnss_localizer.h ++++ b/rubis_ws/src/rubis_pkg/include/gnss_localizer.h +@@ -19,6 +19,7 @@ + #include + #include + #include ++#include + + namespace gnss_localizer + { +diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch b/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch +index 488d479d..3f5a0762 100644 +--- a/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch ++++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch +@@ -2,11 +2,8 @@ + + + +- +- ++ + + +- +- + + +diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch +index 84c50fc6..3f5a0762 100644 +--- a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch ++++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch +@@ -2,10 +2,8 @@ + + + +- +- ++ + + +- + + +diff --git a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch +index 28ae3347..031c4ecc 100644 +--- a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch ++++ b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch +@@ -5,11 +5,8 @@ + + + +- + +- +- +- ++ + + + +diff --git a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch +index 7abb5b98..031c4ecc 100644 +--- a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch ++++ b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch +@@ -5,11 +5,8 @@ + + + +- + +- +- +- ++ + + + +diff --git a/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp b/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp +index 7f094c5d..a7c7a342 100644 +--- a/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp ++++ b/rubis_ws/src/rubis_pkg/src/gnss_localizer.cpp +@@ -70,17 +70,39 @@ void Nmea2TFPoseNode::initForROS() + } + + // setup subscriber +- sub1_ = nh_.subscribe("nmea_sentence", 100, &Nmea2TFPoseNode::callbackFromNmeaSentence, this); +- sub2_ = nh_.subscribe("imu_raw", 100, &Nmea2TFPoseNode::callbackFromIMU, this); ++ sub1_ = nh_.subscribe("nmea_sentence", 1, &Nmea2TFPoseNode::callbackFromNmeaSentence, this); ++ sub2_ = nh_.subscribe("imu_raw", 1, &Nmea2TFPoseNode::callbackFromIMU, this); + + // setup publisher + if(enable_offset_){ +- pub1_ = nh_.advertise("gnss_offset_pose", 10); +- pub2_ = nh_.advertise("gnss_transformed_pose", 10); ++ pub1_ = nh_.advertise("gnss_offset_pose", 1); ++ pub2_ = nh_.advertise("gnss_transformed_pose", 1); + } + else +- pub1_ = nh_.advertise("gnss_pose", 10); +- vel_pub_ = nh_.advertise("gnss_vel", 10); ++ pub1_ = nh_.advertise("gnss_pose", 1); ++ vel_pub_ = nh_.advertise("gnss_vel", 1); ++ ++ // Scheduling & Profiling Setup ++ std::string node_name = ros::this_node::getName(); ++ std::string task_response_time_filename; ++ nh_.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/gnss_localizer.csv"); ++ ++ int rate; ++ nh_.param(node_name+"/rate", rate, 10); ++ ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; ++ ++ nh_.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ nh_.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ nh_.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ nh_.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ nh_.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); ++ ++ rubis::init_task_profiling(task_response_time_filename); + } + + void Nmea2TFPoseNode::run() +diff --git a/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp b/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp +index 8d24cf4e..2a4da217 100644 +--- a/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp ++++ b/rubis_ws/src/rubis_pkg/src/lidar_republisher.cpp +@@ -9,6 +9,8 @@ static ros::Publisher pub, pub_rubis; + int is_topic_ready = 1; + + void points_cb(const sensor_msgs::PointCloud2ConstPtr& msg){ ++ rubis::start_task_profiling(); ++ + sensor_msgs::PointCloud2 msg_with_intensity = *msg; + + msg_with_intensity.fields.at(3).datatype = 7; +@@ -16,15 +18,31 @@ void points_cb(const sensor_msgs::PointCloud2ConstPtr& msg){ + + pub.publish(msg_with_intensity); + +- if(rubis::instance_mode_){ +- rubis_msgs::PointCloud2 rubis_msg_with_intensity; +- rubis_msg_with_intensity.instance = rubis::instance_; +- rubis_msg_with_intensity.msg = msg_with_intensity; +- pub_rubis.publish(rubis_msg_with_intensity); +- } ++ rubis_msgs::PointCloud2 rubis_msg_with_intensity; ++ rubis_msg_with_intensity.instance = rubis::instance_; ++ rubis_msg_with_intensity.msg = msg_with_intensity; ++ pub_rubis.publish(rubis_msg_with_intensity); + +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ rubis::instance_ = rubis::instance_+1; ++ rubis::obj_instance_ = rubis::obj_instance_+1; ++} ++ ++std::string exec(const char* cmd) { ++ char buffer[128]; ++ std::string result = ""; ++ FILE* pipe = popen(cmd, "r"); ++ if (!pipe) throw std::runtime_error("popen() failed!"); ++ try { ++ while (fgets(buffer, sizeof(buffer), pipe) != NULL) { ++ result += buffer; ++ } ++ } catch (...) { ++ pclose(pipe); ++ throw; ++ } ++ pclose(pipe); ++ return result; + } + + int main(int argc, char** argv){ +@@ -39,72 +57,36 @@ int main(int argc, char** argv){ + std::string output_topic_name = node_name + "/output_topic"; + std::string rubis_output_topic; + +- nh.param(node_name+"/instance_mode", rubis::instance_mode_, 0); + nh.param(input_topic_name, input_topic, "/points_raw_origin"); + nh.param(output_topic_name, output_topic, "/points_raw"); + + sub = nh.subscribe(input_topic, 1, points_cb); + pub = nh.advertise(output_topic, 1); +- if(rubis::instance_mode_){ +- rubis_output_topic = "/rubis_"+output_topic.substr(1); +- pub_rubis = nh.advertise(rubis_output_topic, 1); +- } +- +- // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; ++ rubis_output_topic = "/rubis_"+output_topic.substr(1); ++ pub_rubis = nh.advertise(rubis_output_topic, 1); ++ ++ // Scheduling & Profiling Setup + std::string task_response_time_filename; +- int rate; +- double task_minimum_inter_release_time; +- double task_execution_time; +- double task_relative_deadline; ++ private_nh.param(node_name+"/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_republisher.csv"); + +- private_nh.param("/lidar_republisher/task_scheduling_flag", task_scheduling_flag, 0); +- private_nh.param("/lidar_republisher/task_profiling_flag", task_profiling_flag, 0); +- private_nh.param("/lidar_republisher/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/lidar_republisher.csv"); +- private_nh.param("/lidar_republisher/rate", rate, 10); +- private_nh.param("/lidar_republisher/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); +- private_nh.param("/lidar_republisher/task_execution_time", task_execution_time, (double)10); +- private_nh.param("/lidar_republisher/task_relative_deadline", task_relative_deadline, (double)10); +- +- /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); +- +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } ++ int rate; ++ private_nh.param(node_name+"/rate", rate, 10); + +- ros::spinOnce(); ++ struct rubis::sched_attr attr; ++ std::string policy; ++ int priority, exec_time ,deadline, period; + +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); ++ private_nh.param(node_name+"/task_scheduling_configs/policy", policy, std::string("NONE")); ++ private_nh.param(node_name+"/task_scheduling_configs/priority", priority, 99); ++ private_nh.param(node_name+"/task_scheduling_configs/exec_time", exec_time, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/deadline", deadline, 0); ++ private_nh.param(node_name+"/task_scheduling_configs/period", period, 0); ++ attr = rubis::create_sched_attr(priority, exec_time, deadline, period); ++ rubis::init_task_scheduling(policy, attr); + +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- rubis::instance_ = rubis::instance_+1; +- } +- +- +- r.sleep(); +- } +- } ++ rubis::init_task_profiling(task_response_time_filename); ++ ++ ros::spin(); + + return 0; + } +\ No newline at end of file +diff --git a/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp b/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp +index 54e6cd23..a81a3cbd 100644 +--- a/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp ++++ b/rubis_ws/src/rubis_pkg/src/rubis_pose_relay.cpp +@@ -8,20 +8,15 @@ ros::Subscriber rubis_sub_, sub_; + ros::Publisher rubis_pub_, pub_; + + inline void relay(const geometry_msgs::PoseStampedConstPtr& msg){ +- if(rubis::instance_mode_ && rubis::instance_ != RUBIS_NO_INSTANCE){ +- rubis_msgs::PoseStamped rubis_msg; +- rubis_msg.instance = rubis::instance_; +- rubis_msg.msg = *msg; +- rubis_pub_.publish(rubis_msg); +- } ++ rubis_msgs::PoseStamped rubis_msg; ++ rubis_msg.instance = rubis::instance_; ++ rubis_msg.msg = *msg; ++ rubis_pub_.publish(rubis_msg); + pub_.publish(msg); +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; + } + + void cb(const geometry_msgs::PoseStampedConstPtr& msg){ +- rubis::instance_ = RUBIS_NO_INSTANCE; ++ rubis::instance_ = 0; + relay(msg); + } + +@@ -35,8 +30,6 @@ int main(int argc, char* argv[]){ + ros::init(argc, argv, "pose_relay"); + + // Scheduling Setup +- int task_scheduling_flag; +- int task_profiling_flag; + std::string task_response_time_filename; + int rate; + double task_minimum_inter_release_time; +@@ -45,65 +38,34 @@ int main(int argc, char* argv[]){ + + ros::NodeHandle nh; + +- nh.param("/pose_relay/task_scheduling_flag", task_scheduling_flag, 0); +- nh.param("/pose_relay/task_profiling_flag", task_profiling_flag, 0); + nh.param("/pose_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pose_relay.csv"); + nh.param("/pose_relay/rate", rate, 10); + nh.param("/pose_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)10); + nh.param("/pose_relay/task_execution_time", task_execution_time, (double)10); + nh.param("/pose_relay/task_relative_deadline", task_relative_deadline, (double)10); +- nh.param("/pose_relay/instance_mode", rubis::instance_mode_, 0); + + input_topic_ = std::string(argv[1]); + + std::cout<<"!!! input topic "<("/rubis_current_pose", 10); +- } +- else{ +- rubis_sub_ = nh.subscribe(input_topic_, 10, cb); +- } ++ rubis_input_topic_ = "/rubis_"+input_topic_.substr(1); ++ rubis_sub_ = nh.subscribe(rubis_input_topic_, 10, rubis_cb); ++ rubis_pub_ = nh.advertise("/rubis_current_pose", 10); + + pub_ = nh.advertise("/current_pose", 10); + + /* For Task scheduling */ +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); ++ rubis::init_task_profiling(task_response_time_filename); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(rubis::instance_, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } ++ ros::Rate r(rate); ++ while(ros::ok()){ ++ rubis::start_task_profiling(); ++ ++ ros::spinOnce(); ++ ++ rubis::stop_task_profiling(rubis::instance_, 0); ++ ++ r.sleep(); + } + + return 0; +diff --git a/rubis_ws/src/topic_tools/src/relay.cpp b/rubis_ws/src/topic_tools/src/relay.cpp +index 60862697..4a12f860 100644 +--- a/rubis_ws/src/topic_tools/src/relay.cpp ++++ b/rubis_ws/src/topic_tools/src/relay.cpp +@@ -110,10 +110,6 @@ void in_cb(const ros::MessageEvent& msg_event) + } + else + g_pub.publish(msg); +- +- if(rubis::sched::is_task_ready_ == TASK_NOT_READY) rubis::sched::init_task(); +- rubis::sched::task_state_ = TASK_STATE_DONE; +- + } + + void timer_cb(const ros::TimerEvent&) +@@ -184,73 +180,10 @@ int main(int argc, char **argv) + pnh.param("monitor_rate", monitor_rate, 1.0); + monitor_timer = n.createTimer(ros::Duration(monitor_rate), &timer_cb); + } +- +- +- // scheduling +- int task_scheduling_flag = 0; +- int task_profiling_flag = 0; +- std::string task_response_time_filename; +- int rate = 0; +- double task_minimum_inter_release_time = 0; +- double task_execution_time = 0; +- double task_relative_deadline = 0; +- +- if(g_output_topic == std::string("/current_velocity")){ +- pnh.param("/vel_relay/task_scheduling_flag", task_scheduling_flag, 0); +- pnh.param("/vel_relay/task_profiling_flag", task_profiling_flag, 0); +- pnh.param("/vel_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/vel_relay.csv"); +- pnh.param("/vel_relay/rate", rate, 10); +- pnh.param("/vel_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000); +- pnh.param("/vel_relay/task_execution_time", task_execution_time, (double)100000000); +- pnh.param("/vel_relay/task_relative_deadline", task_relative_deadline, (double)100000000); +- } +- else if (g_output_topic == std::string("/current_pose")){ +- pnh.param("/pose_relay/task_scheduling_flag", task_scheduling_flag, 0); +- pnh.param("/pose_relay/task_profiling_flag", task_profiling_flag, 0); +- pnh.param("/pose_relay/task_response_time_filename", task_response_time_filename, "~/Documents/profiling/response_time/pose_relay.csv"); +- pnh.param("/pose_relay/rate", rate, 10); +- pnh.param("/pose_relay/task_minimum_inter_release_time", task_minimum_inter_release_time, (double)100000000); +- pnh.param("/pose_relay/task_execution_time", task_execution_time, (double)100000000); +- pnh.param("/pose_relay/task_relative_deadline", task_relative_deadline, (double)100000000); +- } +- +- if(task_profiling_flag) rubis::sched::init_task_profiling(task_response_time_filename); + + subscribe(); + +- if(!task_scheduling_flag && !task_profiling_flag){ +- ros::spin(); +- } +- else{ +- ros::Rate r(rate); +- // Initialize task ( Wait until first necessary topic is published ) +- while(ros::ok()){ +- if(rubis::sched::is_task_ready_ == TASK_READY) break; +- ros::spinOnce(); +- r.sleep(); +- } +- +- // Executing task +- while(ros::ok()){ +- if(task_profiling_flag) rubis::sched::start_task_profiling(); +- +- if(rubis::sched::task_state_ == TASK_STATE_READY){ +- if(task_scheduling_flag) rubis::sched::request_task_scheduling(task_minimum_inter_release_time, task_execution_time, task_relative_deadline); +- rubis::sched::task_state_ = TASK_STATE_RUNNING; +- } +- +- ros::spinOnce(); +- +- if(task_profiling_flag) rubis::sched::stop_task_profiling(RUBIS_NO_INSTANCE, rubis::sched::task_state_); +- +- if(rubis::sched::task_state_ == TASK_STATE_DONE){ +- if(task_scheduling_flag) rubis::sched::yield_task_scheduling(); +- rubis::sched::task_state_ = TASK_STATE_READY; +- } +- +- r.sleep(); +- } +- } ++ ros::spin(); + + return 0; + } +diff --git a/setup/setup.sh b/setup/setup.sh +index 50df55a6..29fb165c 100755 +--- a/setup/setup.sh ++++ b/setup/setup.sh +@@ -42,21 +42,6 @@ if [ ! -d ~/Documents/profiling/response_time ]; then + printf "~/Documents/profiling/response_time is created.\n" + fi + +-if [ ! -d ~/Documents/gpu_profiling ]; then +- mkdir ~/Documents/gpu_profiling +- printf "~/Documents/gpu_profiling is created.\n" +-fi +- +-if [ ! -d ~/Documents/gpu_profiling ]; then +- mkdir ~/Documents/gpu_profiling +- printf "~/Documents/gpu_profiling is created.\n" +-fi +- +-if [ ! -d ~/Documents/gpu_deadline ]; then +- mkdir ~/Documents/gpu_deadline +- printf "~/Documents/gpu_deadline is created.\n" +-fi +- + echo "Necessary directory paths are created to /home/${1}/Documents" + + sudo ./setup_bashrc.sh $1 diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_1_sensing.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_2_localization.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_3_detection.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_4_planning.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_gpu/_cubetown_autorunner_gpu_5_control.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_1_sensing.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_2_localization.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_3_detection.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_4_planning.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/_cubetown_autorunner_5_control.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/terminate.sh similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/terminate.sh rename to rubis_ws/deprecated/autorunner_scripts/cubetown_autorunner_vgicp/terminate.sh diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch rename to rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_1_sensing.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch rename to rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_2_localization.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch rename to rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_3_planning.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch rename to rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/desktop_lane_keeping_4_control.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/terminate.sh similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/terminate.sh rename to rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/terminate.sh diff --git a/rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/test.launch b/rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/test.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/desktop_lane_keeping/test.launch rename to rubis_ws/deprecated/autorunner_scripts/desktop_lane_keeping/test.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_1_sensing.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_2_localization.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_3_detection.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_4_planning.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_double_curve_autorunner (copy)/_ionic_autorunner_5_control.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_1_sensing.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_2_localization.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_3_detection.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_4_planning.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_gps_test/_ionic_autorunner_5_control.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_1_sensing.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_2_localization.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_3_detection.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_4_planning.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_5_control.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch b/rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch rename to rubis_ws/deprecated/autorunner_scripts/ionic_138ground_single_curve_autorunner/_ionic_autorunner_test.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/_cubetown_autorunner_4_planning.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step1.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step2.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step3.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step4.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step5.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step6.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step7.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step8.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/lgsvl_triple_lidar_step9.launch diff --git a/rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate.sh b/rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/terminate.sh similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/lgsvl_triple_lidar_autorunner/terminate.sh rename to rubis_ws/deprecated/autorunner_scripts/lgsvl_triple_lidar_autorunner/terminate.sh diff --git a/rubis_ws/src/rubis_autorunner/scripts/transformation_test/_transformation_test_1_sensing.launch b/rubis_ws/deprecated/autorunner_scripts/transformation_test/_transformation_test_1_sensing.launch similarity index 100% rename from rubis_ws/src/rubis_autorunner/scripts/transformation_test/_transformation_test_1_sensing.launch rename to rubis_ws/deprecated/autorunner_scripts/transformation_test/_transformation_test_1_sensing.launch diff --git a/rubis_ws/deprecated/gnss_converter/launch/gnss_converter.launch b/rubis_ws/deprecated/gnss_converter/launch/gnss_converter.launch index 658abfef..3f0183c4 100644 --- a/rubis_ws/deprecated/gnss_converter/launch/gnss_converter.launch +++ b/rubis_ws/deprecated/gnss_converter/launch/gnss_converter.launch @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/rubis_ws/deprecated/gnss_converter/launch/gnss_pose_pub.launch b/rubis_ws/deprecated/gnss_converter/launch/gnss_pose_pub.launch index 6a3a3d5b..37522e6b 100644 --- a/rubis_ws/deprecated/gnss_converter/launch/gnss_pose_pub.launch +++ b/rubis_ws/deprecated/gnss_converter/launch/gnss_pose_pub.launch @@ -6,7 +6,7 @@ - + diff --git a/rubis_ws/deprecated/gnss_converter/launch/ntrip_client.launch b/rubis_ws/deprecated/gnss_converter/launch/ntrip_client.launch index 619699c1..45017c90 100644 --- a/rubis_ws/deprecated/gnss_converter/launch/ntrip_client.launch +++ b/rubis_ws/deprecated/gnss_converter/launch/ntrip_client.launch @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/CAN_interface/can_translate/launch/can_translate.launch b/rubis_ws/src/CAN_interface/can_translate/launch/can_translate.launch index 273985d1..671bde01 100644 --- a/rubis_ws/src/CAN_interface/can_translate/launch/can_translate.launch +++ b/rubis_ws/src/CAN_interface/can_translate/launch/can_translate.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch index 6a926615..7ea93364 100755 --- a/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch +++ b/rubis_ws/src/carla_autoware_bridge/launch/carla_autoware_bridge_common.launch @@ -28,14 +28,14 @@ remap carla lidar to autoware. @todo: to reduce load, Autoware should directly use the Carla-topic. --> - + - + - + - + - + diff --git a/rubis_ws/src/carla_autoware_bridge/launch/carla_town04.launch b/rubis_ws/src/carla_autoware_bridge/launch/carla_town04.launch new file mode 100755 index 00000000..deb37689 --- /dev/null +++ b/rubis_ws/src/carla_autoware_bridge/launch/carla_town04.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive b/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive index 3685e92d..69b53989 100755 --- a/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive +++ b/rubis_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/vehiclecmd_to_ackermanndrive @@ -34,9 +34,10 @@ def callback(data): """ global wheelbase msg = AckermannDrive() - msg.speed = data.twist_cmd.twist.linear.x + msg.speed = data.ctrl_cmd.linear_velocity msg.steering_angle = convert_trans_rot_vel_to_steering_angle( msg.speed, data.twist_cmd.twist.angular.z, wheelbase) + msg.acceleration = data.ctrl_cmd.linear_acceleration pub.publish(msg) def twist_to_ackermanndrive(): diff --git a/rubis_ws/src/carla_points_map_loader/launch/carla_points_map_loader.launch b/rubis_ws/src/carla_points_map_loader/launch/carla_points_map_loader.launch index bb22dcb3..0cbd18ab 100755 --- a/rubis_ws/src/carla_points_map_loader/launch/carla_points_map_loader.launch +++ b/rubis_ws/src/carla_points_map_loader/launch/carla_points_map_loader.launch @@ -4,6 +4,6 @@ - + diff --git a/rubis_ws/src/control_module/launch/control_module.launch b/rubis_ws/src/control_module/launch/control_module.launch index 309bfa4e..999ca6af 100644 --- a/rubis_ws/src/control_module/launch/control_module.launch +++ b/rubis_ws/src/control_module/launch/control_module.launch @@ -22,6 +22,6 @@ - + diff --git a/rubis_ws/src/control_module/launch/keyboard_control.launch b/rubis_ws/src/control_module/launch/keyboard_control.launch index 6496af50..0703d177 100644 --- a/rubis_ws/src/control_module/launch/keyboard_control.launch +++ b/rubis_ws/src/control_module/launch/keyboard_control.launch @@ -22,6 +22,6 @@ - + diff --git a/rubis_ws/src/controller/launch/pid_controller.launch b/rubis_ws/src/controller/launch/pid_controller.launch index 18c5f147..48a96bc8 100644 --- a/rubis_ws/src/controller/launch/pid_controller.launch +++ b/rubis_ws/src/controller/launch/pid_controller.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/gicp_localizer/launch/vgicp.launch b/rubis_ws/src/gicp_localizer/launch/vgicp.launch index af122572..7334a2df 100644 --- a/rubis_ws/src/gicp_localizer/launch/vgicp.launch +++ b/rubis_ws/src/gicp_localizer/launch/vgicp.launch @@ -23,7 +23,7 @@ - + diff --git a/rubis_ws/src/gnss_converter/CMakeLists.txt b/rubis_ws/src/gnss_converter/CMakeLists.txt new file mode 100755 index 00000000..8c7a4f57 --- /dev/null +++ b/rubis_ws/src/gnss_converter/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.0.2) +project(gnss_converter) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +set(CMAKE_CXX_FLAGS_RELEASE "-Ofast") + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + inertiallabs_msgs + geometry_msgs + message_filters + tf +) + +find_package(Eigen3 REQUIRED) +set(EIGEN_PACKAGE EIGEN3) +if(NOT EIGEN3_FOUND) + find_package(cmake_modules REQUIRED) + find_package(Eigen REQUIRED) + set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) + set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) + set(EIGEN_PACKAGE Eigen) +endif() + +find_package(OpenCV REQUIRED) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES gnss_converter +# CATKIN_DEPENDS roscpp std_msgs +# DEPENDS system_lib +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + + +add_executable(gnss_converter + include/gnss_converter/gnss_converter.h + include/gnss_converter/LLH2UTM.h + include/gnss_converter/quaternion_euler.h + src/gnss_converter.cpp + src/LLH2UTM.cpp + src/quaternion_euler.cpp +) +add_dependencies(gnss_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gnss_converter + ${catkin_LIBRARIES} + ${EIGEN3_LIBRARIES} + ${OpenCV_LIBS} +) + +add_executable(gnss_pose_pub + include/gnss_converter/LLH2UTM.h + include/gnss_converter/quaternion_euler.h + src/gnss_pose_pub.cpp + src/LLH2UTM.cpp + src/quaternion_euler.cpp +) +add_dependencies(gnss_pose_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gnss_pose_pub + ${catkin_LIBRARIES} + ${EIGEN3_LIBRARIES} + ${OpenCV_LIBS} +) + +install( + TARGETS + gnss_converter + gnss_pose_pub + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + PATTERN ".svn" EXCLUDE +) +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY cfg/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg +) diff --git a/rubis_ws/src/gnss_converter/README.md b/rubis_ws/src/gnss_converter/README.md new file mode 100644 index 00000000..f3594dcb --- /dev/null +++ b/rubis_ws/src/gnss_converter/README.md @@ -0,0 +1,43 @@ +# How to use gnss_converter (/gnss_pose publisher) +### Case 1 : If you create a new map +* Record /Inertial_Labs/gps_data, /Inertial_Labs/ins_data, /ndt_pose, /ndt_stat data while driving through the map (rosbag) +```console +~$ rosbag record /ndt_pose /ndt_stat /Inertial_Labs/gps_data /Inertial_Labs/ins_data -O [NAME] +``` +* Set /gnss_converter/calculate_tf of gnss_converter/cfg/gnsss_converter.yaml to true. +```yaml +/gnss_converter/calculate_tf : true +``` +* Set /gnss_converter/bag_file_path of gnss_converter/cfg/gnsss_converter.yaml to recorded rosbag file. +```yaml +/gnss_converter/bag_file_path : /home/sunhokim/Documents/RUBIS/data/220111_pose_gnss_138ground.bag # example +``` +* Run the following code +```console +~$ roslaunch gnss_converter gnss_converter.launch +``` +* Wait about 40 seconds and a image showing the route of the vehicle will appear on the screen. +* You can select the four points used to calculate the transformation matrix through the following method. + - To change the image size, use track bar to select a value and press enter. + - If you want to choose a point, press the number and click the point in the image. (RBUTTONDOWN) + - If you want to end, press the ESC button. + - **WARNING** : When performing angle transformation, linear transformation was assumed. However, since the angle has a value between -180 degrees and +180 degrees, if there is a discontinuous interval between the four points, the matrix is not properly calculated. Therefore, when selecting four points, it is necessary to make sure that there is no section where the change in angle is discontinuous. (If the first value of the ori_tf matrix is close to -1.0, the calculation was performed well.) +* After the previous process, the transformation matrix calculation result is displayed on the screen. +* The calculation result is transferred to gnss_converter/cfg/gnss_converter.yaml. +```yaml +# ========= position tf matrix ========= +/gnss_converter/pos_tf : [-1.372640, -1.384772, -27.949656, 13944.211735, 0.410968, -0.109160, -11.105078, -9490.840625, -0.318483, -0.086243, -6.272676, 453.012563, 0.000000, -0.000000, 0.000000, 1.000000] + +# ========= orientation tf matrix ========= +/gnss_converter/ori_tf : [-1.000448, 0.345540, 0.569044, 0.987319, 0.021691, -2.279683, -4.681593, -0.067717, 0.056971, 1.608331, -3.444783, -0.114943, 0.000000, -0.000000, 0.000000, 1.000000] +``` +* Move to Case 2 +### Case 2 : If you've already done Case 1 +* Set /gnss_converter/calculate_tf of gnss_converter/cfg/gnsss_converter.yaml to false. +```yaml +/gnss_converter/calculate_tf : false +``` +* Execute gnss_converter launch file. +```console +~$ roslaunch gnss_converter gnss_converter.launch +``` diff --git a/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml b/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml new file mode 100644 index 00000000..fa359036 --- /dev/null +++ b/rubis_ws/src/gnss_converter/cfg/gnss_converter.yaml @@ -0,0 +1,13 @@ +# true : calculate tf matrix +# false : publish /gnss_pose topic +/gnss_converter/calculate_tf : true + +/gnss_converter/bag_file_path : /home/sunhokim/Documents/RUBIS/data/220111_pose_gnss_138ground.bag + +# map : 220111_138ground + +# ========= position tf matrix ========= +/gnss_converter/pos_tf : [-1.372640, -1.384772, -27.949656, 13944.211735, 0.410968, -0.109160, -11.105078, -9490.840625, -0.318483, -0.086243, -6.272676, 453.012563, 0.000000, -0.000000, 0.000000, 1.000000] + +# ========= orientation tf matrix ========= +/gnss_converter/ori_tf : [-1.000448, 0.345540, 0.569044, 0.987319, 0.021691, -2.279683, -4.681593, -0.067717, 0.056971, 1.608331, -3.444783, -0.114943, 0.000000, -0.000000, 0.000000, 1.000000] \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h b/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h new file mode 100644 index 00000000..68728bc5 --- /dev/null +++ b/rubis_ws/src/gnss_converter/include/gnss_converter/LLH2UTM.h @@ -0,0 +1,27 @@ +#include +#include +#include + +#include + +/*************** parameters for LLH to UTM transformation ***************/ + +#define WGS84_A 6378137.0 // major axis +#define WGS84_B 6356752.31424518 // minor axis +#define WGS84_F 0.0033528107 // ellipsoid flattening +#define WGS84_E 0.0818191908 // first eccentricity +#define WGS84_EP 0.0820944379 // second eccentricity + +// UTM Parameters +#define UTM_K0 0.9996 // scale factor +#define UTM_FE 500000.0 // false easting +#define UTM_FN_N 0.0 // false northing, northern hemisphere +#define UTM_FN_S 10000000.0 // false northing, southern hemisphere +#define UTM_E2 (WGS84_E*WGS84_E) // e^2 +#define UTM_E4 (UTM_E2*UTM_E2) // e^4 +#define UTM_E6 (UTM_E4*UTM_E2) // e^6 +#define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2 + +/*************************************************************************/ + +void LLH2UTM(double Lat, double Long, double H, geometry_msgs::PoseStamped& pose); \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h b/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h new file mode 100644 index 00000000..410e74ad --- /dev/null +++ b/rubis_ws/src/gnss_converter/include/gnss_converter/gnss_converter.h @@ -0,0 +1,70 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +using namespace std; +using namespace message_filters; +using namespace Eigen; + +#define ENTER_BUTTON 10 +#define ESC_BUTTON 27 + +struct gps_stat +{ + std_msgs::Header header; + geometry_msgs::Point gps_pose; + geometry_msgs::Vector3 gps_ypr; + geometry_msgs::Point ndt_pose; + geometry_msgs::Vector3 ndt_ypr; + double ndt_score; +}; + +typedef sync_policies::ApproximateTime SyncPolicy_1; +typedef sync_policies::ExactTime SyncPolicy_2; + +static Matrix pos_tf_, ori_tf_; + +static ros::Publisher gnss_pose_pub_; + +static vector gps_backup_; +static int ndt_pose_x_max_ = -9999999, ndt_pose_y_max_ = -9999999; +static int ndt_pose_x_min_ = 9999999, ndt_pose_y_min_ = 9999999; +static int scale_factor_ = 10; + +static int points_idx_; +static gps_stat selected_points_[4]; + +void gps_ndt_data_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins, + const geometry_msgs::PoseStamped::ConstPtr &msg_ndt_pose); +void pub_gnss_pose_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins); +void track_bar_cb(int pos, void *userdata); +void mouse_cb(int event, int x, int y, int flags, void *userdata); +void points_select(); +void calculate_tf_matrix(); \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h b/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h new file mode 100644 index 00000000..50b0c39d --- /dev/null +++ b/rubis_ws/src/gnss_converter/include/gnss_converter/quaternion_euler.h @@ -0,0 +1,7 @@ +#include +#include + +#include + +void ToEulerAngles(geometry_msgs::Quaternion q, double &yaw, double &pitch, double &roll); +void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q); \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/launch/gnss_converter.launch b/rubis_ws/src/gnss_converter/launch/gnss_converter.launch new file mode 100644 index 00000000..658abfef --- /dev/null +++ b/rubis_ws/src/gnss_converter/launch/gnss_converter.launch @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch b/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch new file mode 100644 index 00000000..8cdf1375 --- /dev/null +++ b/rubis_ws/src/gnss_converter/launch/gnss_pose_pub.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/package.xml b/rubis_ws/src/gnss_converter/package.xml new file mode 100644 index 00000000..51cf2b79 --- /dev/null +++ b/rubis_ws/src/gnss_converter/package.xml @@ -0,0 +1,23 @@ + + + gnss_converter + 0.0.0 + The gnss_converter package + + sunhokim + + Apache 2 + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + inertiallabs_msgs + message_filters + tf + eigen + libopencv-dev + diff --git a/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp b/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp new file mode 100644 index 00000000..a39d4698 --- /dev/null +++ b/rubis_ws/src/gnss_converter/src/LLH2UTM.cpp @@ -0,0 +1,54 @@ +#include + +void LLH2UTM(double Lat, double Long, double H, geometry_msgs::PoseStamped& pose){ + double a = WGS84_A; + double eccSquared = UTM_E2; + double k0 = UTM_K0; + double LongOrigin; + double eccPrimeSquared; + double N, T, C, A, M; + // Make sure the longitude is between -180.00 .. 179.9 + // (JOQ: this is broken for Long < -180, do a real normalize) + double LongTemp = (Long+180)-int((Long+180)/360)*360-180; + double LatRad = angles::from_degrees(Lat); + double LongRad = angles::from_degrees(LongTemp); + double LongOriginRad; + pose.pose.position.z = H; + // Fix Zone number with Korea + int zone = 52; + char band = 'S'; + // +3 puts origin in middle of zone + LongOrigin = (zone - 1)*6 - 180 + 3; + LongOriginRad = angles::from_degrees(LongOrigin); + eccPrimeSquared = (eccSquared)/(1-eccSquared); + N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); + T = tan(LatRad)*tan(LatRad); + C = eccPrimeSquared*cos(LatRad)*cos(LatRad); + A = cos(LatRad)*(LongRad-LongOriginRad); + M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 + - 5*eccSquared*eccSquared*eccSquared/256) * LatRad + - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) + + (15*eccSquared*eccSquared/256 + + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) + - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); + pose.pose.position.y = (double) + (k0*N*(A+(1-T+C)*A*A*A/6 + + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) + + 500000.0); + pose.pose.position.x = (double) + (k0*(M+N*tan(LatRad) + *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 + + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); + + double TM[4][4] = + {{-0.821456, -0.593423, -0.006448, 3606301.475406}, + {-0.596954, 0.803991, -0.096993, 2231713.639404}, + {0.049875, 0.018177, -0.047063, -213252.081285}, + {0.000000, 0.000000, 0.000000, 1.000000}}; + + double input[4] = {pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, 1}; + pose.pose.position.x = TM[0][0]*input[0] + TM[0][1]*input[1] + TM[0][2]*input[2] + TM[0][3]*input[3]; + pose.pose.position.y = TM[1][0]*input[0] + TM[1][1]*input[1] + TM[1][2]*input[2] + TM[1][3]*input[3]; + pose.pose.position.z = TM[2][0]*input[0] + TM[2][1]*input[1] + TM[2][2]*input[2] + TM[2][3]*input[3]; +} \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/src/gnss_converter.cpp b/rubis_ws/src/gnss_converter/src/gnss_converter.cpp new file mode 100644 index 00000000..964e49b6 --- /dev/null +++ b/rubis_ws/src/gnss_converter/src/gnss_converter.cpp @@ -0,0 +1,363 @@ +#include +#include +#include + +void gps_ndt_data_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins, + const geometry_msgs::PoseStamped::ConstPtr &msg_ndt_pose) +{ + + geometry_msgs::PoseStamped cur_pose; + + double ndt_yaw, ndt_pitch, ndt_roll; + + LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, cur_pose); + + ToEulerAngles(msg_ndt_pose->pose.orientation, ndt_yaw, ndt_pitch, ndt_roll); + + gps_stat tmp; + geometry_msgs::Vector3 vec_tmp; + + tmp.header = msg_gps->header; + + // gps position & orientation data + tmp.gps_pose = cur_pose.pose.position; + vec_tmp.x = ((msg_ins->YPR.x > 180) ? (msg_ins->YPR.x - 360) : (msg_ins->YPR.x)) / 180 * M_PI; + vec_tmp.y = ((msg_ins->YPR.y > 180) ? (msg_ins->YPR.y - 360) : (msg_ins->YPR.y)) / 180 * M_PI; + vec_tmp.z = ((msg_ins->YPR.z > 180) ? (msg_ins->YPR.z - 360) : (msg_ins->YPR.z)) / 180 * M_PI; + tmp.gps_ypr = vec_tmp; + + // ndt position & orientation data + tmp.ndt_pose = msg_ndt_pose->pose.position; + vec_tmp.x = ndt_yaw; + vec_tmp.y = ndt_pitch; + vec_tmp.z = ndt_roll; + tmp.ndt_ypr = vec_tmp; + + if (ndt_pose_x_max_ < msg_ndt_pose->pose.position.x) + ndt_pose_x_max_ = msg_ndt_pose->pose.position.x; + + if (ndt_pose_x_min_ > msg_ndt_pose->pose.position.x) + ndt_pose_x_min_ = msg_ndt_pose->pose.position.x; + + if (ndt_pose_y_max_ < msg_ndt_pose->pose.position.y) + ndt_pose_y_max_ = msg_ndt_pose->pose.position.y; + + if (ndt_pose_y_min_ > msg_ndt_pose->pose.position.y) + ndt_pose_y_min_ = msg_ndt_pose->pose.position.y; + + gps_backup_.push_back(tmp); +} + +void pub_gnss_pose_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins) +{ + geometry_msgs::PoseStamped cur_pose; + + cur_pose.header = msg_ins->header; + cur_pose.header.frame_id = "/map"; + + Matrix gps; + Matrix ndt; + + /*================= position calculation =================*/ + LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, cur_pose); + + gps(0, 0) = cur_pose.pose.position.x; + gps(1, 0) = cur_pose.pose.position.y; + gps(2, 0) = cur_pose.pose.position.z; + gps(3, 0) = 1.0; + + ndt = pos_tf_ * gps; + + cur_pose.pose.position.x = ndt(0, 0); + cur_pose.pose.position.y = ndt(1, 0); + cur_pose.pose.position.z = ndt(2, 0); + /*=======================================================*/ + + /*=============== orientation calculation ===============*/ + gps(0, 0) = ((msg_ins->YPR.x > 180) ? (msg_ins->YPR.x - 360) : (msg_ins->YPR.x)) / 180 * M_PI; + gps(1, 0) = ((msg_ins->YPR.y > 180) ? (msg_ins->YPR.y - 360) : (msg_ins->YPR.y)) / 180 * M_PI; + gps(2, 0) = ((msg_ins->YPR.z > 180) ? (msg_ins->YPR.z - 360) : (msg_ins->YPR.z)) / 180 * M_PI; + gps(3, 0) = 1.0; + + ndt = ori_tf_ * gps; + + ToQuaternion(ndt(0, 0), ndt(1, 0), ndt(2, 0), cur_pose.pose.orientation); + + gnss_pose_pub_.publish(cur_pose); + /*=======================================================*/ +} + +void scale_image(int pos, void *userdata) +{ + scale_factor_ = ((pos < 1) ? (1) : (pos)); +} + +void mouse_cb(int event, int x, int y, int flags, void *userdata) +{ + if (event == cv::EVENT_RBUTTONDOWN) + { + double point_x = ndt_pose_x_min_ + x / scale_factor_; + double point_y = ndt_pose_y_min_ + y / scale_factor_; + double min_dist = 9999; + int idx; + + for (int i = 0; i < gps_backup_.size(); i++) + { + if (pow((point_x - gps_backup_[i].ndt_pose.x), 2) + pow((point_y - gps_backup_[i].ndt_pose.y), 2) < pow(min_dist, 2)) + { + idx = i; + min_dist = pow((point_x - gps_backup_[i].ndt_pose.x), 2) + pow((point_y - gps_backup_[i].ndt_pose.y), 2); + } + } + + selected_points_[points_idx_] = gps_backup_[idx]; + std::cout << "**************** Point INFO ****************" << std::endl; + + std::cout << "GPS Pose" << std::endl; + std::cout << " x : " << selected_points_[points_idx_].gps_pose.x << std::endl; + std::cout << " y : " << selected_points_[points_idx_].gps_pose.y << std::endl; + std::cout << " z : " << selected_points_[points_idx_].gps_pose.z << std::endl; + + std::cout << "GPS YPR" << std::endl; + std::cout << " yaw : " << selected_points_[points_idx_].gps_ypr.x / M_PI * 180 << std::endl; + std::cout << " pitch : " << selected_points_[points_idx_].gps_ypr.y / M_PI * 180 << std::endl; + std::cout << " roll : " << selected_points_[points_idx_].gps_ypr.z / M_PI * 180 << std::endl; + + std::cout << "NDT Pose" << std::endl; + std::cout << " x : " << selected_points_[points_idx_].ndt_pose.x << std::endl; + std::cout << " y : " << selected_points_[points_idx_].ndt_pose.y << std::endl; + std::cout << " z : " << selected_points_[points_idx_].ndt_pose.z << std::endl; + + std::cout << "NDT YPR" << std::endl; + std::cout << " yaw : " << selected_points_[points_idx_].ndt_ypr.x / M_PI * 180 << std::endl; + std::cout << " pitch : " << selected_points_[points_idx_].ndt_ypr.y / M_PI * 180 << std::endl; + std::cout << " roll : " << selected_points_[points_idx_].ndt_ypr.z / M_PI * 180 << std::endl; + + std::cout << "NDT score" << selected_points_[points_idx_].ndt_score << std::endl; + } +} + +void points_select() +{ + cv::Mat orig_img = cv::Mat((ndt_pose_y_max_ - ndt_pose_y_min_ + 1), (ndt_pose_x_max_ - ndt_pose_x_min_ + 1), CV_8UC3, cv::Scalar(255, 255, 255)); + cv::Mat display_img; + + for (int i = 0; i < gps_backup_.size(); i++) + { + cv::circle(orig_img, cv::Point((gps_backup_[i].ndt_pose.x - ndt_pose_x_min_), (gps_backup_[i].ndt_pose.y - ndt_pose_y_min_)), + 1, cv::Scalar(255, 255, 0), 1, cv::LINE_AA); + } + + orig_img.copyTo(display_img); + + cv::namedWindow("Display Image"); + cv::createTrackbar("Scale", "Display Image", &scale_factor_, 100, scale_image, NULL); + + cv::setMouseCallback("Display Image", mouse_cb, NULL); + + std::cout << "**************** How To Use ****************" << std::endl; + std::cout << "If you want to end, press the ESC button." << std::endl; + std::cout << "If you want to choose a point, press the number and click the point in the image." << std::endl; + std::cout << "To change the image size, use track bar to select a value and press enter." << std::endl; + std::cout << "********************************************" << std::endl; + + int keycode; + while (true) + { + display_img = cv::Mat(orig_img.rows * scale_factor_, orig_img.cols * scale_factor_, CV_8UC3, cv::Scalar(255, 255, 255)); + for (int i = 0; i < gps_backup_.size(); i++) + { + cv::circle(display_img, + cv::Point(scale_factor_ * (gps_backup_[i].ndt_pose.x - ndt_pose_x_min_), scale_factor_ * (gps_backup_[i].ndt_pose.y - ndt_pose_y_min_)), + 1, cv::Scalar(255, 255, 0), 1, cv::LINE_AA); + } + + cv::imshow("Display Image", display_img); + keycode = cv::waitKey(); + + if (keycode == ESC_BUTTON) + break; + + switch (keycode) + { + case '1': + points_idx_ = 0; + std::cout << "Point Number : 1" << std::endl; + break; + case '2': + points_idx_ = 1; + std::cout << "Point Number : 2" << std::endl; + break; + case '3': + points_idx_ = 2; + std::cout << "Point Number : 3" << std::endl; + break; + case '4': + points_idx_ = 3; + std::cout << "Point Number : 4" << std::endl; + break; + case ENTER_BUTTON: + break; + default: + std::cout << "Unknown keyboard input" << std::endl; + break; + } + } +} + +void calculate_tf_matrix() +{ + Matrix gps_pos, ndt_pos, gps_ypr, ndt_ypr; + + for (int i = 0; i < 4; i++) + { + gps_pos(0, i) = selected_points_[i].gps_pose.x; + gps_pos(1, i) = selected_points_[i].gps_pose.y; + gps_pos(2, i) = selected_points_[i].gps_pose.z; + gps_pos(3, i) = 1.0; + + ndt_pos(0, i) = selected_points_[i].ndt_pose.x; + ndt_pos(1, i) = selected_points_[i].ndt_pose.y; + ndt_pos(2, i) = selected_points_[i].ndt_pose.z; + ndt_pos(3, i) = 1.0; + + gps_ypr(0, i) = selected_points_[i].gps_ypr.x; + gps_ypr(1, i) = selected_points_[i].gps_ypr.y; + gps_ypr(2, i) = selected_points_[i].gps_ypr.z; + gps_ypr(3, i) = 1.0; + + ndt_ypr(0, i) = selected_points_[i].ndt_ypr.x; + ndt_ypr(1, i) = selected_points_[i].ndt_ypr.y; + ndt_ypr(2, i) = selected_points_[i].ndt_ypr.z; + ndt_ypr(3, i) = 1.0; + } + + pos_tf_ = ndt_pos * gps_pos.inverse(); + ori_tf_ = ndt_ypr * gps_ypr.inverse(); + + printf("========== position tf matrix ==========\n"); + printf("[%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf]\n", pos_tf_(0, 0), pos_tf_(0, 1), pos_tf_(0, 2), pos_tf_(0, 3), pos_tf_(1, 0), pos_tf_(1, 1), pos_tf_(1, 2), pos_tf_(1, 3), pos_tf_(2, 0), pos_tf_(2, 1), pos_tf_(2, 2), pos_tf_(2, 3), pos_tf_(3, 0), pos_tf_(3, 1), pos_tf_(3, 2), pos_tf_(3, 3)); + printf("======== orientation tf matrix =========\n"); + printf("[%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf]\n", ori_tf_(0, 0), ori_tf_(0, 1), ori_tf_(0, 2), ori_tf_(0, 3), ori_tf_(1, 0), ori_tf_(1, 1), ori_tf_(1, 2), ori_tf_(1, 3), ori_tf_(2, 0), ori_tf_(2, 1), ori_tf_(2, 2), ori_tf_(2, 3), ori_tf_(3, 0), ori_tf_(3, 1), ori_tf_(3, 2), ori_tf_(3, 3)); +} + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "gnss_converter"); + + ros::NodeHandle nh; + + bool calculate_tf; + + gnss_pose_pub_ = nh.advertise("/gnss_pose", 10); + + message_filters::Subscriber gps_sub(nh, "/Inertial_Labs/gps_data", 10); + message_filters::Subscriber ndt_pose_sub(nh, "/ndt_pose", 10); + message_filters::Subscriber ins_sub(nh, "/Inertial_Labs/ins_data", 10); + + Synchronizer sync_1(SyncPolicy_1(100), gps_sub, ins_sub, ndt_pose_sub); + Synchronizer sync_2(SyncPolicy_2(10), gps_sub, ins_sub); + + ros::param::get("/gnss_converter/calculate_tf", calculate_tf); + + if (calculate_tf) + { + sync_1.registerCallback(boost::bind(&gps_ndt_data_cb, _1, _2, _3)); + + pid_t pid; + if ((pid = fork()) < 0) + ROS_ERROR("Cannot create child!"); + + if (pid == 0) + { + std::string file_path; + char file_path_cstr[150]; + + ros::param::get("/gnss_converter/bag_file_path", file_path); + + int str_len = file_path.length(); + if (str_len >= 150) + { + ROS_ERROR("File path is too long!!"); + exit(0); + } + + strcpy(file_path_cstr, file_path.c_str()); + + const char *file = "/opt/ros/melodic/bin/rosbag"; + char *exe_argv[] = {"/opt/ros/melodic/bin/rosbag", + "play", + "-r", + "5", + file_path_cstr, + NULL}; + + if (execvp(file, exe_argv) < 0) + { + ROS_ERROR("Cannot load bag file!!"); + } + } + else + { + int wstatus; + while (waitpid(pid, &wstatus, WNOHANG) == 0) + { + ros::spinOnce(); + } + ROS_WARN("Finish loading data from rosbag file!!"); + points_select(); + calculate_tf_matrix(); + } + } + + else + { + sync_2.registerCallback(boost::bind(&pub_gnss_pose_cb, _1, _2)); + + vector tf_tmp; + + /*================= pos_tf_ matrix =================*/ + ros::param::get("/gnss_converter/pos_tf", tf_tmp); + pos_tf_(0, 0) = tf_tmp[0]; + pos_tf_(0, 1) = tf_tmp[1]; + pos_tf_(0, 2) = tf_tmp[2]; + pos_tf_(0, 3) = tf_tmp[3]; + pos_tf_(1, 0) = tf_tmp[4]; + pos_tf_(1, 1) = tf_tmp[5]; + pos_tf_(1, 2) = tf_tmp[6]; + pos_tf_(1, 3) = tf_tmp[7]; + pos_tf_(2, 0) = tf_tmp[8]; + pos_tf_(2, 1) = tf_tmp[9]; + pos_tf_(2, 2) = tf_tmp[10]; + pos_tf_(2, 3) = tf_tmp[11]; + pos_tf_(3, 0) = tf_tmp[12]; + pos_tf_(3, 1) = tf_tmp[13]; + pos_tf_(3, 2) = tf_tmp[14]; + pos_tf_(3, 3) = tf_tmp[15]; + /*=================================================*/ + + /*================= ori_tf_ matrix =================*/ + ros::param::get("/gnss_converter/ori_tf", tf_tmp); + ori_tf_(0, 0) = tf_tmp[0]; + ori_tf_(0, 1) = tf_tmp[1]; + ori_tf_(0, 2) = tf_tmp[2]; + ori_tf_(0, 3) = tf_tmp[3]; + ori_tf_(1, 0) = tf_tmp[4]; + ori_tf_(1, 1) = tf_tmp[5]; + ori_tf_(1, 2) = tf_tmp[6]; + ori_tf_(1, 3) = tf_tmp[7]; + ori_tf_(2, 0) = tf_tmp[8]; + ori_tf_(2, 1) = tf_tmp[9]; + ori_tf_(2, 2) = tf_tmp[10]; + ori_tf_(2, 3) = tf_tmp[11]; + ori_tf_(3, 0) = tf_tmp[12]; + ori_tf_(3, 1) = tf_tmp[13]; + ori_tf_(3, 2) = tf_tmp[14]; + ori_tf_(3, 3) = tf_tmp[15]; + /*================================================*/ + + ros::spin(); + } + + return 0; +} \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp b/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp new file mode 100644 index 00000000..ad248edd --- /dev/null +++ b/rubis_ws/src/gnss_converter/src/gnss_pose_pub.cpp @@ -0,0 +1,91 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#define M_PI 3.14159265358979323846 + +using namespace std; +using namespace message_filters; + +typedef sync_policies::ExactTime SyncPolicy_; + +static ros::Publisher gnss_pose_pub_; +static double x_offset_, y_offset_, yaw_offset_; +static double roll_, pitch_, yaw_; +static geometry_msgs::PoseStamped gnss_pose_; + +void gnss_pose_pub_cb(const inertiallabs_msgs::gps_data::ConstPtr &msg_gps, const inertiallabs_msgs::ins_data::ConstPtr &msg_ins){ + gnss_pose_.header = msg_gps->header; + gnss_pose_.header.frame_id = "/map"; + + LLH2UTM(msg_gps->LLH.x, msg_gps->LLH.y, msg_gps->LLH.z, gnss_pose_); + + gnss_pose_.pose.position.x = gnss_pose_.pose.position.x - x_offset_; + gnss_pose_.pose.position.y = gnss_pose_.pose.position.y - y_offset_; + + roll_ = msg_ins->YPR.z; + pitch_ = msg_ins->YPR.y; + + yaw_ = msg_ins->YPR.x; + yaw_ *= -1; + yaw_ -= yaw_offset_; + if(yaw_ > 180.0) yaw_ -= 360.0; + if(yaw_ < -180.0) yaw_ += 360.0; + + roll_ = roll_ * M_PI/180.0; + pitch_ = pitch_ * M_PI/180.0; + yaw_ = yaw_ * M_PI/180.0; +} + +int main(int argc, char *argv[]){ + ros::init(argc, argv, "gnss_pose_pub"); + + ros::NodeHandle nh; + + nh.param("/gnss_pose_pub/x_offset", x_offset_, 0.0); + nh.param("/gnss_pose_pub/y_offset", y_offset_, 0.0); + nh.param("/ins_twist_generator/yaw_offset", yaw_offset_, 0.0); + + gnss_pose_pub_ = nh.advertise("/ndt_pose", 2); + + message_filters::Subscriber gps_sub(nh, "/Inertial_Labs/gps_data", 2); + message_filters::Subscriber ins_sub(nh, "/Inertial_Labs/ins_data", 2); + + Synchronizer sync(SyncPolicy_(2), gps_sub, ins_sub); + + sync.registerCallback(boost::bind(&gnss_pose_pub_cb, _1, _2)); + + tf::TransformBroadcaster br; + ros::Rate r(10); + while(nh.ok()){ + ros::spinOnce(); + + ToQuaternion(yaw_, pitch_, roll_, gnss_pose_.pose.orientation); + + tf::Quaternion q; + q.setRPY(roll_, pitch_, yaw_); + + tf::StampedTransform transform; + transform.setOrigin(tf::Vector3(gnss_pose_.pose.position.x, gnss_pose_.pose.position.y, 0.0)); + transform.setRotation(q); + br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/map", "/base_link")); + + gnss_pose_pub_.publish(gnss_pose_); + + + r.sleep(); + } +} \ No newline at end of file diff --git a/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp b/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp new file mode 100644 index 00000000..158c979b --- /dev/null +++ b/rubis_ws/src/gnss_converter/src/quaternion_euler.cpp @@ -0,0 +1,32 @@ +#include + +void ToEulerAngles(geometry_msgs::Quaternion q, double &yaw, double &pitch, double &roll){ + double sinr_cosp = 2 * (q.w * q.x + q.y * q.z); + double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y); + roll = std::atan2(sinr_cosp, cosr_cosp); + + double sinp = 2 * (q.w * q.y - q.z * q.x); + if (std::abs(sinp) >= 1) + pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range + else + pitch = std::asin(sinp); + + double siny_cosp = 2 * (q.w * q.z + q.x * q.y); + double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); + yaw = std::atan2(siny_cosp, cosy_cosp); +} + +void ToQuaternion(double yaw, double pitch, double roll, geometry_msgs::Quaternion &q) +{ + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + + q.w = cr * cp * cy + sr * sp * sy; + q.x = sr * cp * cy - cr * sp * sy; + q.y = cr * sp * cy + sr * cp * sy; + q.z = cr * cp * sy - sr * sp * cy; +} \ No newline at end of file diff --git a/rubis_ws/src/gnss_module/launch/gnss_module.launch b/rubis_ws/src/gnss_module/launch/gnss_module.launch index bd06f459..e00550d9 100644 --- a/rubis_ws/src/gnss_module/launch/gnss_module.launch +++ b/rubis_ws/src/gnss_module/launch/gnss_module.launch @@ -19,7 +19,7 @@ - + diff --git a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch b/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch index dea85d01..baff29f4 100644 --- a/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch +++ b/rubis_ws/src/inertiallabs_pkgs/inertiallabs_ins/launch/ins.launch @@ -1,5 +1,5 @@ - + diff --git a/rubis_ws/src/ins_twist_generator/launch/dynamic_ins_twist_generator.launch b/rubis_ws/src/ins_twist_generator/launch/dynamic_ins_twist_generator.launch index 2a5bfb3d..e0fe478f 100644 --- a/rubis_ws/src/ins_twist_generator/launch/dynamic_ins_twist_generator.launch +++ b/rubis_ws/src/ins_twist_generator/launch/dynamic_ins_twist_generator.launch @@ -2,7 +2,7 @@ - + diff --git a/rubis_ws/src/ins_twist_generator/launch/ins_twist_generator.launch b/rubis_ws/src/ins_twist_generator/launch/ins_twist_generator.launch index 34481d62..20d44825 100644 --- a/rubis_ws/src/ins_twist_generator/launch/ins_twist_generator.launch +++ b/rubis_ws/src/ins_twist_generator/launch/ins_twist_generator.launch @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml index c49e3f72..40654dad 100755 --- a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml +++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml @@ -15,4 +15,4 @@ carla_ackermann_control: # at more or less constant speed. # If the absolute value of the ackermann drive target acceleration exceeds this value, # directly the input acceleration is controlled - min_accel: 1.0 + min_accel: 0.005 diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup new file mode 100755 index 00000000..c49e3f72 --- /dev/null +++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/config/settings.yaml.backup @@ -0,0 +1,18 @@ +carla_ackermann_control: + ros__parameters: + # override the default values of the pid speed controller + # (only relevant for ackermann control mode) + speed_Kp: 0.05 # min: 0, max: 1. + speed_Ki: 0.00 # min: 0, max: 1. + speed_Kd: 0.50 # min: 0, max: 10. + # override the default values of the pid acceleration controller + # (only relevant for ackermann control mode) + accel_Kp: 0.05 # min: 0, max: 10. + accel_Ki: 0.00 # min: 0, max: 10. + accel_Kd: 0.05 # min: 0, max: 10. + # set the minimum acceleration in (m/s^2) + # This border value is used to enable the speed controller which is used to control driving + # at more or less constant speed. + # If the absolute value of the ackermann drive target acceleration exceeds this value, + # directly the input acceleration is controlled + min_accel: 1.0 diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/launch/carla_ackermann_control.launch b/rubis_ws/src/ros-bridge/carla_ackermann_control/launch/carla_ackermann_control.launch index 6428956c..fe431c14 100755 --- a/rubis_ws/src/ros-bridge/carla_ackermann_control/launch/carla_ackermann_control.launch +++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/launch/carla_ackermann_control.launch @@ -11,7 +11,7 @@ - + @@ -23,5 +23,5 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py b/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py index 944f70fd..c8d29c00 100755 --- a/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py +++ b/rubis_ws/src/ros-bridge/carla_ackermann_control/src/carla_ackermann_control/carla_ackermann_control_node.py @@ -442,6 +442,7 @@ def run_speed_control_loop(self): else: if self.info.status.speed_control_activation_count > 0: self.info.status.speed_control_activation_count -= 1 + # set the auto_mode of the controller accordingly self.speed_controller.auto_mode = self.info.status.speed_control_activation_count >= 5 diff --git a/rubis_ws/src/ros-bridge/carla_ad_agent/launch/carla_ad_agent.launch b/rubis_ws/src/ros-bridge/carla_ad_agent/launch/carla_ad_agent.launch index 71b78102..81ae04df 100755 --- a/rubis_ws/src/ros-bridge/carla_ad_agent/launch/carla_ad_agent.launch +++ b/rubis_ws/src/ros-bridge/carla_ad_agent/launch/carla_ad_agent.launch @@ -10,12 +10,12 @@ - + - + diff --git a/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo.launch.py b/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo.launch.py index 19336f89..e382bef2 100755 --- a/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo.launch.py +++ b/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo.launch.py @@ -29,7 +29,7 @@ def launch_target_speed_publisher(context, *args, **kwargs): data_string = "{'data': " + launch.substitutions.LaunchConfiguration('target_speed').perform(context) + "}" return [ launch.actions.ExecuteProcess( - output="screen", + , cmd=["ros2", "topic", "pub", topic_name, "std_msgs/msg/Float64", data_string, "--qos-durability", "transient_local"], name='topic_pub_target_speed')] diff --git a/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch b/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch index b1ade608..924ec473 100755 --- a/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch +++ b/rubis_ws/src/ros-bridge/carla_ad_demo/launch/carla_ad_demo_with_scenario.launch @@ -70,7 +70,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_fake_control/launch/carla_fake_control.launch b/rubis_ws/src/ros-bridge/carla_fake_control/launch/carla_fake_control.launch index 438dc6f3..efe346d2 100755 --- a/rubis_ws/src/ros-bridge/carla_fake_control/launch/carla_fake_control.launch +++ b/rubis_ws/src/ros-bridge/carla_fake_control/launch/carla_fake_control.launch @@ -2,7 +2,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_manual_control/launch/carla_manual_control.launch b/rubis_ws/src/ros-bridge/carla_manual_control/launch/carla_manual_control.launch index 6a38db62..f8273a34 100755 --- a/rubis_ws/src/ros-bridge/carla_manual_control/launch/carla_manual_control.launch +++ b/rubis_ws/src/ros-bridge/carla_manual_control/launch/carla_manual_control.launch @@ -2,7 +2,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge.launch b/rubis_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge.launch index 4bc9aa82..1cddf412 100755 --- a/rubis_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge.launch +++ b/rubis_ws/src/ros-bridge/carla_ros_bridge/launch/carla_ros_bridge.launch @@ -27,7 +27,7 @@ --> - + diff --git a/rubis_ws/src/ros-bridge/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch b/rubis_ws/src/ros-bridge/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch index 0c465164..093b97c3 100755 --- a/rubis_ws/src/ros-bridge/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch +++ b/rubis_ws/src/ros-bridge/carla_ros_scenario_runner/launch/carla_ros_scenario_runner.launch @@ -6,7 +6,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json index 9aef8118..d31f2c9f 100755 --- a/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json +++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/config/objects.json @@ -55,13 +55,14 @@ "type": "sensor.lidar.ray_cast", "id": "lidar", "spawn_point": {"x": 0.0, "y": 0.0, "z": 2.4, "roll": 0.0, "pitch": 0.0, "yaw": 0.0}, - "range": 50, - "channels": 32, + "range": 100, + "channels": 16, "points_per_second": 320000, "upper_fov": 2.0, "lower_fov": -26.8, "rotation_frequency": 20, - "noise_stddev": 0.0 + "noise_stddev": 0.0, + "sensor_tick": 0.1 }, { "type": "sensor.other.gnss", diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_spawn_objects.launch b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_spawn_objects.launch index 885ffdde..ef6347ff 100755 --- a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_spawn_objects.launch +++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/carla_spawn_objects.launch @@ -9,7 +9,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/set_initial_pose.launch b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/set_initial_pose.launch index ba4647b6..ffc62e27 100755 --- a/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/set_initial_pose.launch +++ b/rubis_ws/src/ros-bridge/carla_spawn_objects/launch/set_initial_pose.launch @@ -4,7 +4,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_twist_to_control/launch/carla_twist_to_control.launch b/rubis_ws/src/ros-bridge/carla_twist_to_control/launch/carla_twist_to_control.launch index d3694776..71b74e99 100755 --- a/rubis_ws/src/ros-bridge/carla_twist_to_control/launch/carla_twist_to_control.launch +++ b/rubis_ws/src/ros-bridge/carla_twist_to_control/launch/carla_twist_to_control.launch @@ -2,7 +2,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_walker_agent/launch/carla_walker_agent.launch b/rubis_ws/src/ros-bridge/carla_walker_agent/launch/carla_walker_agent.launch index a107b701..b33f0bc1 100644 --- a/rubis_ws/src/ros-bridge/carla_walker_agent/launch/carla_walker_agent.launch +++ b/rubis_ws/src/ros-bridge/carla_walker_agent/launch/carla_walker_agent.launch @@ -8,7 +8,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/carla_waypoint_publisher/launch/carla_waypoint_publisher.launch b/rubis_ws/src/ros-bridge/carla_waypoint_publisher/launch/carla_waypoint_publisher.launch index 1fdbedf3..e3ef70a1 100644 --- a/rubis_ws/src/ros-bridge/carla_waypoint_publisher/launch/carla_waypoint_publisher.launch +++ b/rubis_ws/src/ros-bridge/carla_waypoint_publisher/launch/carla_waypoint_publisher.launch @@ -5,7 +5,7 @@ - + diff --git a/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch b/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch index 9df6d76c..9a3bd358 100644 --- a/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch +++ b/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch @@ -9,7 +9,7 @@ args="pub -l /carla/$(arg role_name)/enable_autopilot std_msgs/Bool '{ data: true}' " /> - + diff --git a/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch.py b/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch.py index d2510032..919416b3 100644 --- a/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch.py +++ b/rubis_ws/src/ros-bridge/pcl_recorder/launch/pcl_recorder.launch.py @@ -10,7 +10,7 @@ def launch_enable_autopilot_publisher(context, *args, **kwargs): topic_name = "/carla/" + launch.substitutions.LaunchConfiguration('role_name').perform(context) + "/enable_autopilot" return [ launch.actions.ExecuteProcess( - output="screen", + , cmd=["ros2", "topic", "pub", topic_name, "std_msgs/msg/Bool", "{'data': True}", "--qos-durability", "transient_local"], name='topic_pub_enable_autopilot')] diff --git a/rubis_ws/src/rubis_autorunner/CMakeLists.txt b/rubis_ws/src/rubis_autorunner/CMakeLists.txt index 2b0febb6..6c3f20a5 100644 --- a/rubis_ws/src/rubis_autorunner/CMakeLists.txt +++ b/rubis_ws/src/rubis_autorunner/CMakeLists.txt @@ -48,16 +48,40 @@ add_executable(minicar_lane_keeping src/minicar_lane_keeping/minicar_lane_keeping.cpp ) -add_executable(cubetown_autorunner +add_executable(carla_lkas_autorunner + include/carla_autorunner/carla_autorunner.h + src/carla_autorunner/carla_lkas_autorunner_node.cpp + src/carla_autorunner/carla_lkas_autorunner.cpp +) + +add_executable(carla_full_autorunner + include/carla_autorunner/carla_autorunner.h + src/carla_autorunner/carla_full_autorunner_node.cpp + src/carla_autorunner/carla_full_autorunner.cpp +) + +add_executable(exynos_carla_lkas_autorunner + include/carla_autorunner/carla_autorunner.h + src/carla_autorunner/exynos_carla_lkas_autorunner_node.cpp + src/carla_autorunner/exynos_carla_lkas_autorunner.cpp +) + +add_executable(exynos_carla_full_autorunner + include/carla_autorunner/carla_autorunner.h + src/carla_autorunner/exynos_carla_full_autorunner_node.cpp + src/carla_autorunner/exynos_carla_full_autorunner.cpp +) + +add_executable(cubetown_lkas_autorunner include/cubetown_autorunner/cubetown_autorunner.h - src/cubetown_autorunner/cubetown_autorunner_node.cpp - src/cubetown_autorunner/cubetown_autorunner.cpp + src/cubetown_autorunner/cubetown_lkas_autorunner_node.cpp + src/cubetown_autorunner/cubetown_lkas_autorunner.cpp ) -add_executable(cubetown_autorunner_gpu +add_executable(cubetown_full_autorunner include/cubetown_autorunner/cubetown_autorunner.h - src/cubetown_autorunner/cubetown_autorunner_gpu_node.cpp - src/cubetown_autorunner/cubetown_autorunner_gpu.cpp + src/cubetown_autorunner/cubetown_full_autorunner_node.cpp + src/cubetown_autorunner/cubetown_full_autorunner.cpp ) add_executable(tutorial_autorunner @@ -90,18 +114,44 @@ target_link_libraries(minicar_lane_keeping ros_autorunner_lib ) -add_dependencies(cubetown_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(cubetown_autorunner +add_dependencies(cubetown_lkas_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(cubetown_lkas_autorunner + ${catkin_LIBRARIES} + ros_autorunner_lib +) + +add_dependencies(cubetown_full_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(cubetown_full_autorunner + ${catkin_LIBRARIES} + ros_autorunner_lib +) + +add_dependencies(carla_lkas_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(carla_lkas_autorunner + ${catkin_LIBRARIES} + ros_autorunner_lib +) + +add_dependencies(carla_full_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(carla_full_autorunner ${catkin_LIBRARIES} ros_autorunner_lib ) -add_dependencies(cubetown_autorunner_gpu ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(cubetown_autorunner_gpu +add_dependencies(exynos_carla_lkas_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(exynos_carla_lkas_autorunner ${catkin_LIBRARIES} ros_autorunner_lib ) +add_dependencies(exynos_carla_full_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(exynos_carla_full_autorunner + ${catkin_LIBRARIES} + ros_autorunner_lib +) + + + add_dependencies(tutorial_autorunner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(tutorial_autorunner ${catkin_LIBRARIES} diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml deleted file mode 100644 index 1680abe6..00000000 --- a/rubis_ws/src/rubis_autorunner/cfg/carla/params.yaml +++ /dev/null @@ -1,206 +0,0 @@ -# Unit of rate: hz -# Unit of task information: nano seconds - -# 1s : 1000000000 -# 1ms : 1000000 -# 1us : 1000 -# 1ns : 1 - -#rate = hz -# other time unut = ns - -# Sensing -ray_ground_filter: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" - rate: 10 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - -# Localization -voxel_grid_filter: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" - rate: 10 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - -ndt_matching: - localizer: "velodyne" - - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" - rate: 100 - task_minimum_inter_release_time: 100000000 - task_execution_time: 70000000 - task_relative_deadline: 100000000 - gpu_scheduling_flag: 0 - gpu_profiling_flag: 0 - gpu_execution_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_execution_time.csv" - gpu_response_time_filename: "~/Documents/gpu_profiling/test_ndt_matching_response_time.csv" - gpu_deadline_filename: "~/Documents/gpu_deadline/ndt_matching_gpu_deadline.csv" - -logger_brake_dist: - rate: 10 - dist_filename: "~/Documents/profiling/response_time/dist.csv" - -# Detection -calibration_publisher: - calibration_file: "~/autoware.ai/autoware_files/data/calibration/single_lidar/Lexus2016RXHybrid.yaml" - -lidar_euclidean_cluster_detect: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" - rate: 10 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - gpu_scheduling_flag: 0 - gpu_profiling_flag: 0 - gpu_execution_time_filename: "~/Documents/gpu_profiling/test_clustering_execution_time.csv" - gpu_response_time_filename: "~/Documents/gpu_profiling/test_clustering_response_time.csv" - gpu_deadline_filename: "~/Documents/gpu_deadline/clustring_gpu_deadline.csv" - -vision_darknet_detect: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/vision_darknet_detect.csv" - rate: 100 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - gpu_scheduling_flag: 0 - gpu_profiling_flag: 0 - gpu_execution_time_filename: "~/Documents/gpu_profiling/test_yolo_execution_time.csv" - gpu_response_time_filename: "~/Documents/gpu_profiling/test_yolo_response_time.csv" - gpu_deadline_filename: "~/Documents/gpu_deadline/yolo_gpu_deadline.csv" - network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" - pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3.weights" - -imm_ukf_pda_track: - task_scheduling_flag: 0 - task_profiling_flag: 0 - task_response_time_filename: "~/Documents/profiling/response_time/imm_ukf_pda_track.csv" - rate: 10 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - -range_vision_fusion: - task_scheduling_flag: 0 - task_profiling_flag: 0 - task_response_time_filename: "~/Documents/profiling/response_time/range_vision_fusion.csv" - rate: 10 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - -# Planning -op_global_planner: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" - rate: 25 #25 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - multilap_flag: 0 - multilap_replanning_distance : 50 - -op_common_params: - rollOutDensity: 4 - rollOutsNumber: 2 - maxVelocity: 10.0 - maxAcceleration: 10.0 - maxDeceleration: -10.0 - -op_trajectory_generator: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" - rate: 100 #100 - task_minimum_inter_release_time: 10000000 - task_execution_time: 10000000 - task_relative_deadline: 10000000 - -op_trajectory_evaluator: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" - rate: 100 #100 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - weightPriority: 0 - weightTransition: 5 - weightLong: 4 - weightLat: 4 - ImageWidth: 400 - ImageHeight: 400 - SprintDecisionTime: 9999999.0 - PedestrianStopImgHeightThreshold: 20 - PedestrianImageDetectionRange: 0.4 - # PedestrianRightThreshold: 1.0 - # PedestrianLeftThreshold: 1.0 - # VehicleImageDetectionRange: 1.0 - # VehicleImageWidthThreshold: 1.0 - - -op_behavior_selector: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" - rate: 100 #100 - task_minimum_inter_release_time: 10000000 - task_execution_time: 10000000 - task_relative_deadline: 10000000 - distanceToPedestrianThreshold: 15.0 - sprintSpeed: 10.0 - obstacleWaitingTimeinIntersection: 2.0 - turnThreshold: 30.0 - -op_motion_predictor: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" - rate: 25 #25 - task_minimum_inter_release_time: 40000000 - task_execution_time: 40000000 - task_relative_deadline: 40000000 - -# Control -pure_pursuit: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" - rate: 30 #30 - task_minimum_inter_release_time: 33333333 - task_execution_time: 33333333 - task_relative_deadline: 33333333 - dynamic_params_flag: False - dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" - -twist_filter: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" - rate: 10 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - -twist_gate: - task_scheduling_flag: 0 - task_profiling_flag: 1 - task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" - rate: 10 - task_minimum_inter_release_time: 100000000 - task_execution_time: 100000000 - task_relative_deadline: 100000000 - zero_flag: 0 diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml new file mode 100644 index 00000000..a50f843b --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_autorunner_params.yaml @@ -0,0 +1,184 @@ +# Unit of rate: hz +# Unit of scheduling params: ns + +# 1s : 1_000_000_000 +# 1ms : 1_000_000 +# 1us : 1_000 +# 1ns : 1 + +#rate = hz +# other time unut = ns + +# Lane Keeping +lidar_republisher: + rate: 10 + task_scheduling_configs: + policy: "NONE" + priority: 99 + exec_time: 1_000_000 + deadline: 2_000_000 + period: 2_000_000 + task_response_time_filename: "~/Documents/profiling/response_time/lidar_republisher.csv" + +voxel_grid_filter: + rate: 10 + task_scheduling_configs: + policy: "NONE" + priority: 99 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/voxel_grid_filter.csv" + +ndt_matching: + rate: 10 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/ndt_matching.csv" + + use_kalman_filter: False + tf_x: 1.0510799 + tf_y: 0 + tf_z: 1.96 + tf_roll: 0 + tf_pitch: 0 + tf_yaw: 0 + localizer: "velodyne" + +pure_pursuit: + rate: 30 #30 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/pure_pursuit.csv" + + dynamic_params_flag: False + dynamic_params_path: "~/autoware.ai/autoware_files/lgsvl_file/parameter/lgsvl_pure_pursuit.yaml" + +twist_filter: + rate: 10 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/twist_filter.csv" + +# Detection +ray_ground_filter_center: + rate: 10 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/ray_ground_filter.csv" + +lidar_euclidean_cluster_detect: + rate: 10 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/lidar_euclidean_cluster_detect.csv" + +vision_darknet_detect: + network_definition_file: "~/autoware.ai/autoware_files/vision/yolov3-320.cfg" + pretrained_model_file: "~/autoware.ai/autoware_files/vision/yolov3.weights" + +# Planning +op_global_planner: + rate: 25 #25 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/op_global_planner.csv" + + multilap_flag: 1 + +op_common_params: + rollOutDensity: 4 + rollOutsNumber: 2 + maxVelocity: 8.0 + maxAcceleration: 8.0 + maxDeceleration: -8.0 + +op_trajectory_generator: + rate: 20 #100 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_generator.csv" + +op_trajectory_evaluator: + rate: 100 #100 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/op_trajectory_evaluator.csv" + + weightPriority: 1 + weightTransition: 0.5 + weightLong: 1 + weightLat: 1 + ImageWidth: 1920 + ImageHeight: 1080 + SprintDecisionTime: 9999999.0 + +op_behavior_selector: + rate: 100 #100 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/op_behavior_selector.csv" + + distanceToPedestrianThreshold: 15.0 + sprintSpeed: 10.0 + obstacleWaitingTimeinIntersection: 2.0 + turnThreshold: 30.0 + +op_motion_predictor: + rate: 25 #25 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/op_motion_predictor.csv" + +# Independent +twist_gate: + rate: 10 + task_scheduling_configs: + policy: "NONE" + priority: 20 + exec_time: 0 # ns + deadline: 0 # ns + period: 0 # ns + task_response_time_filename: "~/Documents/profiling/response_time/twist_gate.csv" + + zero_flag: 0 ## Publish target velocity as 0 \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml new file mode 100644 index 00000000..83c242dd --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_full_autorunner.yaml @@ -0,0 +1,14 @@ +total_step_num: 5 +terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh" +# step_# : +# [0] pacakge name +# [1] target name( node name or launch script name ) +# [2] state that create constraint topic or not ( "true" or "false" ) +# [3] state that check constraint topic or not ( "true" or "false" ) +# [4] target type( RUN or LAUNCH ) + +step_1: ["rubis_autorunner", "_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"] +step_2: ["rubis_autorunner", "_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"] +step_3: ["rubis_autorunner", "_carla_autorunner_3_detection.launch", "true", "true", "LAUNCH"] +step_4: ["rubis_autorunner", "_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"] +step_5: ["rubis_autorunner", "_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"] diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml new file mode 100644 index 00000000..6fed9fc7 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_lkas_autorunner.yaml @@ -0,0 +1,13 @@ +total_step_num: 4 +terminate_script_path: "~/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/terminate_desktop.sh" +# step_# : +# [0] pacakge name +# [1] target name( node name or launch script name ) +# [2] state that create constraint topic or not ( "true" or "false" ) +# [3] state that check constraint topic or not ( "true" or "false" ) +# [4] target type( RUN or LAUNCH ) + +step_1: ["rubis_autorunner", "_carla_autorunner_1_sensing.launch", "true", "false", "LAUNCH"] +step_2: ["rubis_autorunner", "_carla_autorunner_2_localization.launch", "true", "true", "LAUNCH"] +step_3: ["rubis_autorunner", "_carla_autorunner_4_planning.launch", "true", "true", "LAUNCH"] +step_4: ["rubis_autorunner", "_carla_autorunner_5_control.launch", "false", "true", "LAUNCH"] diff --git a/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_spawn_points.yaml b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_spawn_points.yaml new file mode 100644 index 00000000..c0dc2441 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/cfg/carla_autorunner/carla_spawn_points.yaml @@ -0,0 +1,68 @@ +# Unit of rate: hz +# Unit of scheduling params: ns + +# 1s : 1_000_000_000 +# 1ms : 1_000_000 +# 1us : 1_000 +# 1ns : 1 + +#rate = hz +# other time unut = ns + +# Lane Keeping +ego_vehicle: + spawn_points: + x: 314.065 + y: 129.676 + z: 0.3 + roll: 0 + pitch: 0 + yaw: 90 + +obstacle: + enable: True + spawn_points: + x: 315.8 + y: 192.664 + z: 0.3 + roll: 0 + pitch: 0 + yaw: 90 + + # + # + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch new file mode 100644 index 00000000..e1d2ee10 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization.launch @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch new file mode 100644 index 00000000..df533eb6 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_2_localization_gicp.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch new file mode 100644 index 00000000..cbfe177e --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_3_detection.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch new file mode 100644 index 00000000..571980c0 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_4_planning.launch @@ -0,0 +1,118 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch new file mode 100644 index 00000000..59144340 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_carla_autorunner_5_control.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_1_sensing.launch new file mode 100644 index 00000000..49b200e5 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_1_sensing.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_2_localization.launch new file mode 100644 index 00000000..e1d2ee10 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_2_localization.launch @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_3_detection.launch new file mode 100644 index 00000000..ef383011 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_3_detection.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_4_planning.launch new file mode 100644 index 00000000..571980c0 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_4_planning.launch @@ -0,0 +1,118 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_5_control.launch b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_5_control.launch new file mode 100644 index 00000000..59144340 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/carla_autorunner/_exynos_carla_autorunner_5_control.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch index 2f156bae..fdf35425 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_1_sensing.launch @@ -29,28 +29,14 @@ - - - - - - - - + - - - - - - - + - - + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch index 44835cc4..8afcb8a1 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_2_localization.launch @@ -36,17 +36,11 @@ pitch: $(arg init_pitch), yaw: $(arg init_yaw), use_predict_pose: 1, - error_threshold: 0.01, - resolution: 3.0, - step_size: 0.3, + error_threshold: 0.05, + resolution: 1.0, + step_size: 0.5, trans_epsilon: 0.01, - max_iterations: 10} + max_iterations: 2} '"/> - - - - - - diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_3_detection.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_3_detection.launch index 66594cf2..7c9bced5 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_3_detection.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_3_detection.launch @@ -53,34 +53,12 @@ - + - - - - - - - - - - - - - - - - - - - - + --> \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch index ee35d158..819a2793 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_4_planning.launch @@ -67,8 +67,8 @@ - - + + @@ -82,11 +82,18 @@ + + + + + + + diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_5_control.origin.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_5_control.origin.launch new file mode 100644 index 00000000..b50bd926 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_cubetown_autorunner_5_control.origin.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch index 68b2046e..6fbbc3a2 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_modular_single_lidar_2_localization.launch @@ -15,7 +15,6 @@ - diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch index d7dd14fc..b399ee97 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_1_sensing.launch @@ -19,26 +19,21 @@ - - - + - - - @@ -53,7 +48,7 @@ - + diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch index e7d3fd19..05ed4386 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/_multi_lidar_2_localizer.launch @@ -15,7 +15,6 @@ - @@ -33,7 +32,6 @@ - @@ -78,10 +76,9 @@ - - + @@ -123,7 +120,6 @@ - @@ -161,7 +157,7 @@ - + ["ndt_pose_FL", "ndt_pose_FR", "ndt_pose_B"] ["ndt_stat_FL", "ndt_stat_FR", "ndt_stat_B"] diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch index a9efdf71..546fc8d7 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner/test.launch @@ -31,27 +31,23 @@ - + - + - - - - + - @@ -76,7 +72,6 @@ - diff --git a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/terminate.sh b/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/terminate.sh deleted file mode 100644 index 242e6f2c..00000000 --- a/rubis_ws/src/rubis_autorunner/scripts/cubetown_autorunner_vgicp/terminate.sh +++ /dev/null @@ -1 +0,0 @@ -rosnode kill /cubetown \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_gps_red_course/_ionic_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_gps_red_course/_ionic_autorunner_1_sensing.launch index 37db9886..340132b4 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_gps_red_course/_ionic_autorunner_1_sensing.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_gps_red_course/_ionic_autorunner_1_sensing.launch @@ -22,7 +22,7 @@ - + @@ -42,7 +42,7 @@ diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch index 76fdc686..44a69ccc 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_gicp_localizer.launch @@ -15,7 +15,6 @@ - @@ -23,7 +22,6 @@ - @@ -35,7 +33,7 @@ - + diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch index 2a317ad2..c71066ba 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_2_modular_localizer.launch @@ -14,7 +14,6 @@ - @@ -32,7 +31,6 @@ - diff --git a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_4_planning.launch b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_4_planning.launch index be3db144..f3b7d877 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_4_planning.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/ionic_FMTC_red_course/_ionic_autorunner_4_planning.launch @@ -20,7 +20,7 @@ - + diff --git a/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/_rubis_testbed_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/_rubis_testbed_autorunner_1_sensing.launch index 8929284e..97eddb56 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/_rubis_testbed_autorunner_1_sensing.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/rubis_testbed_autorunner/_rubis_testbed_autorunner_1_sensing.launch @@ -22,7 +22,7 @@ - + @@ -35,7 +35,7 @@ diff --git a/rubis_ws/src/rubis_autorunner/scripts/tutorial_autorunner/_tutorial_autorunner_1_sensing.launch b/rubis_ws/src/rubis_autorunner/scripts/tutorial_autorunner/_tutorial_autorunner_1_sensing.launch index 9393c304..bac37403 100644 --- a/rubis_ws/src/rubis_autorunner/scripts/tutorial_autorunner/_tutorial_autorunner_1_sensing.launch +++ b/rubis_ws/src/rubis_autorunner/scripts/tutorial_autorunner/_tutorial_autorunner_1_sensing.launch @@ -22,9 +22,9 @@ - + - + @@ -35,7 +35,7 @@ diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp new file mode 100644 index 00000000..b0891ca0 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner.cpp @@ -0,0 +1,122 @@ +#include "carla_autorunner/carla_autorunner.h" + +void CarlaAutorunner::Run(){ + register_subscribers(); // Register subscribers that shoud check can go next or not + ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner + ros::Rate rate(1); // Rate can be changed + while(ros::ok()){ + if(!ros_autorunner_.Run()) break; // Run Autorunner + ros::spinOnce(); + geometry_msgs::PoseStamped goal_msg; + goal_msg.header.stamp = ros::Time::now(); + goal_msg.header.frame_id = "world"; + goal_msg.pose.position.x = 302.250; + goal_msg.pose.position.y = 118.089; + goal_msg.pose.position.z = 0.0; + goal_msg.pose.orientation.x = 0.0; + goal_msg.pose.orientation.y = 0.0; + goal_msg.pose.orientation.z = -0.015; + goal_msg.pose.orientation.w = 1.000; + + goal_pub_.publish(goal_msg); + rate.sleep(); + } +} + +void CarlaAutorunner::register_subscribers(){ + int total_step_num = nh_.param("/total_step_num", -1); + if(total_step_num < 0){ + std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); + goal_pub_ = nh_.advertise("/move_base_simple/goal", 1); +} + + void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ + if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ + ROS_WARN("[STEP 1] Map and Sensors are prepared"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; + } + } + + void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ + static int failure_cnt = 0, success_cnt = 0; + failure_cnt++; + + static const double pos_x = 314.072479248; + static const double pos_y = 129.654495239; + static const double pos_z = 0.044597864151; + + static const double ori_x = 0.0; + static const double ori_y = 0.0; + static const double ori_z = 0.70715665039; + static const double ori_w = 0.70705690407; + + + if(failure_cnt > 10){ + std::cout<<"# Refresh inital pose"<= pos_x - 1.5 && + msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 && + msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 && + msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 && + msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 && + msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 && + !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ + success_cnt++; + if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } + else{ + success_cnt = 0; + } + } + +void CarlaAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){ + if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){ + ROS_WARN("[STEP 3] All detection modules are excuted"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; + } +} + + + void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ + std::string state = msg.markers.front().text; + + if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ + ROS_WARN("[STEP 4] Global & local planning success"); + ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true; + } +} + + + diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp new file mode 100644 index 00000000..caac332f --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_full_autorunner_node.cpp @@ -0,0 +1,11 @@ +#include + +int main(int argc, char* argv[]){ + ros::init(argc, argv, "carla_autorunner"); + ros::NodeHandle nh; + + CarlaAutorunner carla_autorunner(nh); + carla_autorunner.Run(); + + return 0; +} \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp new file mode 100644 index 00000000..2258ce99 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner.cpp @@ -0,0 +1,111 @@ +#include "carla_autorunner/carla_autorunner.h" + +void CarlaAutorunner::Run(){ + register_subscribers(); // Register subscribers that shoud check can go next or not + ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner + ros::Rate rate(1); // Rate can be changed + while(ros::ok()){ + if(!ros_autorunner_.Run()) break; // Run Autorunner + ros::spinOnce(); + geometry_msgs::PoseStamped goal_msg; + goal_msg.header.stamp = ros::Time::now(); + goal_msg.header.frame_id = "world"; + goal_msg.pose.position.x = 302.250; + goal_msg.pose.position.y = 118.089; + goal_msg.pose.position.z = 0.0; + goal_msg.pose.orientation.x = 0.0; + goal_msg.pose.orientation.y = 0.0; + goal_msg.pose.orientation.z = -0.015; + goal_msg.pose.orientation.w = 1.000; + + rate.sleep(); + } +} + +void CarlaAutorunner::register_subscribers(){ + int total_step_num = nh_.param("/total_step_num", -1); + if(total_step_num < 0){ + std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); + goal_pub_ = nh_.advertise("/move_base_simple/goal", 1); +} + + void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ + if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ + ROS_WARN("[STEP 1] Map and Sensors are prepared"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; + } + } + + void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ + static int failure_cnt = 0, success_cnt = 0; + failure_cnt++; + + static const double pos_x = 314.072479248; + static const double pos_y = 129.654495239; + static const double pos_z = 0.044597864151; + + static const double ori_x = 0.0; + static const double ori_y = 0.0; + static const double ori_z = 0.70715665039; + static const double ori_w = 0.70705690407; + + + if(failure_cnt > 10){ + std::cout<<"# Refresh inital pose"<= pos_x - 1.5 && + msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 && + msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 && + msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 && + msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 && + msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 && + !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ + success_cnt++; + if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } + else{ + success_cnt = 0; + } + } + + + void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ + std::string state = msg.markers.front().text; + if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ + ROS_WARN("[STEP 3] Global & local planning success"); + ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; + } +} + + + diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp new file mode 100644 index 00000000..caac332f --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/carla_lkas_autorunner_node.cpp @@ -0,0 +1,11 @@ +#include + +int main(int argc, char* argv[]){ + ros::init(argc, argv, "carla_autorunner"); + ros::NodeHandle nh; + + CarlaAutorunner carla_autorunner(nh); + carla_autorunner.Run(); + + return 0; +} \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner.cpp new file mode 100644 index 00000000..b0891ca0 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner.cpp @@ -0,0 +1,122 @@ +#include "carla_autorunner/carla_autorunner.h" + +void CarlaAutorunner::Run(){ + register_subscribers(); // Register subscribers that shoud check can go next or not + ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner + ros::Rate rate(1); // Rate can be changed + while(ros::ok()){ + if(!ros_autorunner_.Run()) break; // Run Autorunner + ros::spinOnce(); + geometry_msgs::PoseStamped goal_msg; + goal_msg.header.stamp = ros::Time::now(); + goal_msg.header.frame_id = "world"; + goal_msg.pose.position.x = 302.250; + goal_msg.pose.position.y = 118.089; + goal_msg.pose.position.z = 0.0; + goal_msg.pose.orientation.x = 0.0; + goal_msg.pose.orientation.y = 0.0; + goal_msg.pose.orientation.z = -0.015; + goal_msg.pose.orientation.w = 1.000; + + goal_pub_.publish(goal_msg); + rate.sleep(); + } +} + +void CarlaAutorunner::register_subscribers(){ + int total_step_num = nh_.param("/total_step_num", -1); + if(total_step_num < 0){ + std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); + goal_pub_ = nh_.advertise("/move_base_simple/goal", 1); +} + + void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ + if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ + ROS_WARN("[STEP 1] Map and Sensors are prepared"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; + } + } + + void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ + static int failure_cnt = 0, success_cnt = 0; + failure_cnt++; + + static const double pos_x = 314.072479248; + static const double pos_y = 129.654495239; + static const double pos_z = 0.044597864151; + + static const double ori_x = 0.0; + static const double ori_y = 0.0; + static const double ori_z = 0.70715665039; + static const double ori_w = 0.70705690407; + + + if(failure_cnt > 10){ + std::cout<<"# Refresh inital pose"<= pos_x - 1.5 && + msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 && + msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 && + msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 && + msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 && + msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 && + !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ + success_cnt++; + if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } + else{ + success_cnt = 0; + } + } + +void CarlaAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){ + if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){ + ROS_WARN("[STEP 3] All detection modules are excuted"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; + } +} + + + void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ + std::string state = msg.markers.front().text; + + if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ + ROS_WARN("[STEP 4] Global & local planning success"); + ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true; + } +} + + + diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner_node.cpp new file mode 100644 index 00000000..caac332f --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_full_autorunner_node.cpp @@ -0,0 +1,11 @@ +#include + +int main(int argc, char* argv[]){ + ros::init(argc, argv, "carla_autorunner"); + ros::NodeHandle nh; + + CarlaAutorunner carla_autorunner(nh); + carla_autorunner.Run(); + + return 0; +} \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner.cpp new file mode 100644 index 00000000..2258ce99 --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner.cpp @@ -0,0 +1,111 @@ +#include "carla_autorunner/carla_autorunner.h" + +void CarlaAutorunner::Run(){ + register_subscribers(); // Register subscribers that shoud check can go next or not + ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner + ros::Rate rate(1); // Rate can be changed + while(ros::ok()){ + if(!ros_autorunner_.Run()) break; // Run Autorunner + ros::spinOnce(); + geometry_msgs::PoseStamped goal_msg; + goal_msg.header.stamp = ros::Time::now(); + goal_msg.header.frame_id = "world"; + goal_msg.pose.position.x = 302.250; + goal_msg.pose.position.y = 118.089; + goal_msg.pose.position.z = 0.0; + goal_msg.pose.orientation.x = 0.0; + goal_msg.pose.orientation.y = 0.0; + goal_msg.pose.orientation.z = -0.015; + goal_msg.pose.orientation.w = 1.000; + + rate.sleep(); + } +} + +void CarlaAutorunner::register_subscribers(){ + int total_step_num = nh_.param("/total_step_num", -1); + if(total_step_num < 0){ + std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); + goal_pub_ = nh_.advertise("/move_base_simple/goal", 1); +} + + void CarlaAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ + if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ + ROS_WARN("[STEP 1] Map and Sensors are prepared"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; + } + } + + void CarlaAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ + static int failure_cnt = 0, success_cnt = 0; + failure_cnt++; + + static const double pos_x = 314.072479248; + static const double pos_y = 129.654495239; + static const double pos_z = 0.044597864151; + + static const double ori_x = 0.0; + static const double ori_y = 0.0; + static const double ori_z = 0.70715665039; + static const double ori_w = 0.70705690407; + + + if(failure_cnt > 10){ + std::cout<<"# Refresh inital pose"<= pos_x - 1.5 && + msg.pose.position.y <= pos_y + 1.5 && msg.pose.position.y >= pos_y - 1.5 && + msg.pose.orientation.x <= ori_x + 0.01 && msg.pose.orientation.x >= ori_x - 0.01 && + msg.pose.orientation.y <= ori_y + 0.01 && msg.pose.orientation.y >= ori_y - 0.01 && + msg.pose.orientation.z <= ori_z + 0.01 && msg.pose.orientation.z >= ori_z - 0.01 && + msg.pose.orientation.w <= ori_w + 0.01 && msg.pose.orientation.w >= ori_w - 0.01 && + !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ + success_cnt++; + if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } + else{ + success_cnt = 0; + } + } + + + void CarlaAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ + std::string state = msg.markers.front().text; + if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ + ROS_WARN("[STEP 3] Global & local planning success"); + ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; + } +} + + + diff --git a/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner_node.cpp new file mode 100644 index 00000000..caac332f --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/carla_autorunner/exynos_carla_lkas_autorunner_node.cpp @@ -0,0 +1,11 @@ +#include + +int main(int argc, char* argv[]){ + ros::init(argc, argv, "carla_autorunner"); + ros::NodeHandle nh; + + CarlaAutorunner carla_autorunner(nh); + carla_autorunner.Run(); + + return 0; +} \ No newline at end of file diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu.cpp deleted file mode 100644 index 0f28c375..00000000 --- a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include "cubetown_autorunner/cubetown_autorunner.h" - -void CubetownAutorunner::Run(){ - register_subscribers(); // Register subscribers that shoud check can go next or not - ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner - ros::Rate rate(1); // Rate can be changed - while(ros::ok()){ - ros_autorunner_.Run(); // Run Autorunner - ros::spinOnce(); - rate.sleep(); - } -} - -void CubetownAutorunner::register_subscribers(){ - sub_v_.resize(TOTAL_STEP_NUM); // Resizing the subscriber vectors. Its size must be same with number of steps - - // Set the check function(subscriber) - sub_v_[STEP(1)] = nh_.subscribe("/points_raw", 1, &CubetownAutorunner::points_raw_cb, this); - sub_v_[STEP(2)] = nh_.subscribe("/ndt_stat", 1, &CubetownAutorunner::ndt_stat_cb, this); - sub_v_[STEP(3)] = nh_.subscribe("/detection/object_tracker/objects_center", 1, &CubetownAutorunner::detection_cb, this); - sub_v_[STEP(4)] = nh_.subscribe("/behavior_state", 1, &CubetownAutorunner::behavior_state_cb, this); -} - - void CubetownAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ - if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ - ROS_WARN("[STEP 1] Map and Sensors are prepared"); - sleep(SLEEP_PERIOD); - ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; - ROS_WARN("[STEP 2] check is skiped"); - ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; - } - } - - void CubetownAutorunner::ndt_stat_cb(const autoware_msgs::NDTStat& msg){ - - } - -void CubetownAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){ - if(!msg.objects.empty() && !ros_autorunner_.step_info_list_[STEP(4)].is_prepared){ - ROS_WARN("[STEP 3] All detection modules are excuted"); - sleep(SLEEP_PERIOD); - ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; - } -} - - - void CubetownAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ - std::string state = msg.markers.front().text; - if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ - ROS_WARN("[STEP 4] Global & local planning success"); - ros_autorunner_.step_info_list_[STEP(5)].is_prepared = true; - } -} - - - diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp similarity index 52% rename from rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner.cpp rename to rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp index 2be9d167..2e1e95ef 100644 --- a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner.cpp +++ b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner.cpp @@ -5,20 +5,27 @@ void CubetownAutorunner::Run(){ ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner ros::Rate rate(1); // Rate can be changed while(ros::ok()){ - ros_autorunner_.Run(); // Run Autorunner + if(!ros_autorunner_.Run()) break; // Run Autorunner ros::spinOnce(); rate.sleep(); } } void CubetownAutorunner::register_subscribers(){ - sub_v_.resize(TOTAL_STEP_NUM); // Resizing the subscriber vectors. Its size must be same with number of steps + int total_step_num = nh_.param("/total_step_num", -1); + if(total_step_num < 0){ + std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); } void CubetownAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ @@ -29,12 +36,38 @@ void CubetownAutorunner::register_subscribers(){ } } - void CubetownAutorunner::ndt_stat_cb(const autoware_msgs::NDTStat& msg){ - if(msg.score < 0.2 && !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ + void CubetownAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ + static int failure_cnt = 0, success_cnt = 0; + failure_cnt++; + + if(failure_cnt > 10){ + std::cout<<"# Refresh inital pose"<= 55.0 && + msg.pose.position.y >= -1.0 && msg.pose.position.y <= 1.0 && + !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ + success_cnt++; + if(success_cnt < 3) return; ROS_WARN("[STEP 2] Localization is success"); sleep(SLEEP_PERIOD); ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; } + else{ + success_cnt = 0; + } + } void CubetownAutorunner::detection_cb(const autoware_msgs::DetectedObjectArray& msg){ diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu_node.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner_node.cpp similarity index 100% rename from rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_gpu_node.cpp rename to rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_full_autorunner_node.cpp diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp new file mode 100644 index 00000000..4b18747a --- /dev/null +++ b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner.cpp @@ -0,0 +1,82 @@ +#include "cubetown_autorunner/cubetown_autorunner.h" + +void CubetownAutorunner::Run(){ + register_subscribers(); // Register subscribers that shoud check can go next or not + ros_autorunner_.init(nh_, sub_v_); // Initialize the ROS-Autorunner + ros::Rate rate(1); // Rate can be changed + while(ros::ok()){ + if(!ros_autorunner_.Run()) break; // Run Autorunner + ros::spinOnce(); + rate.sleep(); + } +} + +void CubetownAutorunner::register_subscribers(){ + int total_step_num = nh_.param("/total_step_num", -1); + if(total_step_num < 0){ + std::cout<<"Parameter total_step_num is invalid"<("initialpose", 1); +} + + void CubetownAutorunner::points_raw_cb(const sensor_msgs::PointCloud2& msg){ + if(!msg.fields.empty() && !ros_autorunner_.step_info_list_[STEP(2)].is_prepared){ + ROS_WARN("[STEP 1] Map and Sensors are prepared"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(2)].is_prepared = true; + } + } + + void CubetownAutorunner::ndt_pose_cb(const geometry_msgs::PoseStamped& msg){ + static int failure_cnt = 0, success_cnt = 0; + failure_cnt++; + + if(failure_cnt > 10){ + std::cout<<"# Refresh inital pose"<= 55.0 && + msg.pose.position.y >= -1.0 && msg.pose.position.y <= 1.00 && + !ros_autorunner_.step_info_list_[STEP(3)].is_prepared){ + success_cnt++; + if(success_cnt < 3) return; + ROS_WARN("[STEP 2] Localization is success"); + sleep(SLEEP_PERIOD); + ros_autorunner_.step_info_list_[STEP(3)].is_prepared = true; + } + else{ + success_cnt = 0; + } + + } + + + void CubetownAutorunner::behavior_state_cb(const visualization_msgs::MarkerArray& msg){ + std::string state = msg.markers.front().text; + if(!msg.markers.empty() && state.find(std::string("Forward"))!=std::string::npos){ + ROS_WARN("[STEP 3] Global & local planning success"); + ros_autorunner_.step_info_list_[STEP(4)].is_prepared = true; + } +} + + + diff --git a/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_node.cpp b/rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner_node.cpp similarity index 100% rename from rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_autorunner_node.cpp rename to rubis_ws/src/rubis_autorunner/src/cubetown_autorunner/cubetown_lkas_autorunner_node.cpp diff --git a/rubis_ws/src/rubis_autorunner/src/ros_autorunner_lib/ros_autorunner.cpp b/rubis_ws/src/rubis_autorunner/src/ros_autorunner_lib/ros_autorunner.cpp index 4d489736..84ac22a5 100755 --- a/rubis_ws/src/rubis_autorunner/src/ros_autorunner_lib/ros_autorunner.cpp +++ b/rubis_ws/src/rubis_autorunner/src/ros_autorunner_lib/ros_autorunner.cpp @@ -83,16 +83,16 @@ void ROSAutorunner::init(ros::NodeHandle nh, Sub_v sub_v){ print_step_info_list(); } -void ROSAutorunner::Run(){ +bool ROSAutorunner::Run(){ if(current_step_ == step_info_list_.end()){ - return; + return false; } if(!current_step_->check_topic) current_step_->is_prepared = true; if(current_step_->is_prepared == true){ - ROS_WARN("[Step %d] Activated", INT_TO_STEP(current_step_->step_id)); + ROS_WARN("[STEP %d] Activated", INT_TO_STEP(current_step_->step_id)); if(current_step_->target_type == RUN) run_node(current_step_->step_id); else if(current_step_->target_type == LAUNCH) @@ -109,7 +109,7 @@ void ROSAutorunner::Run(){ ++current_step_; } - return; + return true; } void ROSAutorunner::run_node(int step_id){ diff --git a/rubis_ws/src/rubis_logger/launch/logger_brake_dist.launch b/rubis_ws/src/rubis_logger/launch/logger_brake_dist.launch index 7f40bcec..48fe81af 100644 --- a/rubis_ws/src/rubis_logger/launch/logger_brake_dist.launch +++ b/rubis_ws/src/rubis_logger/launch/logger_brake_dist.launch @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/rubis_logger/launch/logger_brake_dist_param.launch b/rubis_ws/src/rubis_logger/launch/logger_brake_dist_param.launch index f00c63fe..531d9ccf 100644 --- a/rubis_ws/src/rubis_logger/launch/logger_brake_dist_param.launch +++ b/rubis_ws/src/rubis_logger/launch/logger_brake_dist_param.launch @@ -1,3 +1,3 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/rubis_logger/launch/logger_topic.launch b/rubis_ws/src/rubis_logger/launch/logger_topic.launch index d47780a6..5debc56f 100644 --- a/rubis_ws/src/rubis_logger/launch/logger_topic.launch +++ b/rubis_ws/src/rubis_logger/launch/logger_topic.launch @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/rubis_pkg/launch/TF_test.launch b/rubis_ws/src/rubis_pkg/launch/TF_test.launch index f2841242..7049372e 100644 --- a/rubis_ws/src/rubis_pkg/launch/TF_test.launch +++ b/rubis_ws/src/rubis_pkg/launch/TF_test.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/rubis_pkg/launch/camera_republisher.launch b/rubis_ws/src/rubis_pkg/launch/camera_republisher.launch index 34a86ee4..bd74a93c 100644 --- a/rubis_ws/src/rubis_pkg/launch/camera_republisher.launch +++ b/rubis_ws/src/rubis_pkg/launch/camera_republisher.launch @@ -1,6 +1,6 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/fake_current_pose.launch b/rubis_ws/src/rubis_pkg/launch/fake_current_pose.launch index a113a3a8..1d32a27b 100644 --- a/rubis_ws/src/rubis_pkg/launch/fake_current_pose.launch +++ b/rubis_ws/src/rubis_pkg/launch/fake_current_pose.launch @@ -1,3 +1,3 @@ - + \ No newline at end of file diff --git a/rubis_ws/src/rubis_pkg/launch/fake_object_generator.launch b/rubis_ws/src/rubis_pkg/launch/fake_object_generator.launch index f42898e1..c5d294a5 100644 --- a/rubis_ws/src/rubis_pkg/launch/fake_object_generator.launch +++ b/rubis_ws/src/rubis_pkg/launch/fake_object_generator.launch @@ -1,4 +1,4 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/fake_traffic_signal_generator.launch b/rubis_ws/src/rubis_pkg/launch/fake_traffic_signal_generator.launch index da907d56..bf6eac7c 100644 --- a/rubis_ws/src/rubis_pkg/launch/fake_traffic_signal_generator.launch +++ b/rubis_ws/src/rubis_pkg/launch/fake_traffic_signal_generator.launch @@ -1,7 +1,7 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/gnss_localizer.launch b/rubis_ws/src/rubis_pkg/launch/gnss_localizer.launch index 58bac3ba..3ba4e8c4 100644 --- a/rubis_ws/src/rubis_pkg/launch/gnss_localizer.launch +++ b/rubis_ws/src/rubis_pkg/launch/gnss_localizer.launch @@ -1,7 +1,7 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/gnss_republisher.launch b/rubis_ws/src/rubis_pkg/launch/gnss_republisher.launch index 0a3123be..68446dc3 100644 --- a/rubis_ws/src/rubis_pkg/launch/gnss_republisher.launch +++ b/rubis_ws/src/rubis_pkg/launch/gnss_republisher.launch @@ -1,6 +1,6 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/lgsvl_traffic_signal_test.launch b/rubis_ws/src/rubis_pkg/launch/lgsvl_traffic_signal_test.launch index 8aecf02d..c273eccf 100644 --- a/rubis_ws/src/rubis_pkg/launch/lgsvl_traffic_signal_test.launch +++ b/rubis_ws/src/rubis_pkg/launch/lgsvl_traffic_signal_test.launch @@ -1,6 +1,6 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch b/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch index fa7019a5..488d479d 100644 --- a/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch +++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher.launch @@ -3,7 +3,7 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch index 59639703..84c50fc6 100644 --- a/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch +++ b/rubis_ws/src/rubis_pkg/launch/lidar_republisher_params.launch @@ -3,7 +3,7 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/twist_cmd_publisher.launch b/rubis_ws/src/rubis_pkg/launch/twist_cmd_publisher.launch index 87052115..3873fba7 100644 --- a/rubis_ws/src/rubis_pkg/launch/twist_cmd_publisher.launch +++ b/rubis_ws/src/rubis_pkg/launch/twist_cmd_publisher.launch @@ -1,6 +1,6 @@ - + diff --git a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch index cb72d0b5..28ae3347 100644 --- a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch +++ b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect.launch @@ -7,10 +7,10 @@ - + - + diff --git a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch index d2272d1d..7abb5b98 100644 --- a/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch +++ b/rubis_ws/src/rubis_pkg/launch/vel_pose_connect_params.launch @@ -7,10 +7,10 @@ - + - + diff --git a/rubis_ws/src/topic_tools/test/delete_mux.test b/rubis_ws/src/topic_tools/test/delete_mux.test index 524b47ce..90feebad 100644 --- a/rubis_ws/src/topic_tools/test/delete_mux.test +++ b/rubis_ws/src/topic_tools/test/delete_mux.test @@ -1,7 +1,7 @@ + args="a b c"/> diff --git a/rubis_ws/src/topic_tools/test/mux.test b/rubis_ws/src/topic_tools/test/mux.test index 0c49e162..4007f803 100644 --- a/rubis_ws/src/topic_tools/test/mux.test +++ b/rubis_ws/src/topic_tools/test/mux.test @@ -5,7 +5,7 @@ args="pub -r 5 input2 std_msgs/String input2"/> - diff --git a/rubis_ws/src/topic_tools/test/mux_add.test b/rubis_ws/src/topic_tools/test/mux_add.test index f29d1260..5c5e0d29 100644 --- a/rubis_ws/src/topic_tools/test/mux_add.test +++ b/rubis_ws/src/topic_tools/test/mux_add.test @@ -2,7 +2,7 @@ - diff --git a/rubis_ws/src/topic_tools/test/mux_initial_none.test b/rubis_ws/src/topic_tools/test/mux_initial_none.test index 954956fd..72efcfba 100644 --- a/rubis_ws/src/topic_tools/test/mux_initial_none.test +++ b/rubis_ws/src/topic_tools/test/mux_initial_none.test @@ -5,7 +5,7 @@ args="pub -r 5 input2 std_msgs/String input2"/> - diff --git a/rubis_ws/src/topic_tools/test/mux_initial_other.test b/rubis_ws/src/topic_tools/test/mux_initial_other.test index 6a1738c6..f027d5df 100644 --- a/rubis_ws/src/topic_tools/test/mux_initial_other.test +++ b/rubis_ws/src/topic_tools/test/mux_initial_other.test @@ -5,7 +5,7 @@ args="pub -r 5 input2 std_msgs/String input2"/> - diff --git a/rubis_ws/src/topic_tools/test/throttle_simtime_loop.test b/rubis_ws/src/topic_tools/test/throttle_simtime_loop.test index a4556a2f..6a3a43b7 100644 --- a/rubis_ws/src/topic_tools/test/throttle_simtime_loop.test +++ b/rubis_ws/src/topic_tools/test/throttle_simtime_loop.test @@ -3,7 +3,7 @@ + args="messages input 5"/> diff --git a/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch b/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch index d0b2ea9d..7857f4e8 100644 --- a/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch +++ b/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_node.launch @@ -8,7 +8,7 @@ + launch-prefix="$(arg launch_prefix)" > diff --git a/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch b/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch index 5060fe34..a5c3843c 100644 --- a/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch +++ b/rubis_ws/src/vesc/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch @@ -17,7 +17,7 @@ + args="manager" launch-prefix="$(arg launch_prefix)"> diff --git a/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch b/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch index a2cf374c..48644eeb 100644 --- a/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch +++ b/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_node.launch @@ -8,7 +8,7 @@ + launch-prefix="$(arg launch_prefix)" > diff --git a/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch b/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch index 19a3aab2..1db0a9cf 100644 --- a/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch +++ b/rubis_ws/src/vesc/vesc_ackermann/launch/vesc_to_odom_nodelet.launch @@ -17,7 +17,7 @@ + args="manager" launch-prefix="$(arg launch_prefix)"> diff --git a/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_node.launch b/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_node.launch index ddcf9b9b..ec15f0d2 100644 --- a/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_node.launch +++ b/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_node.launch @@ -13,7 +13,7 @@ + launch-prefix="$(arg launch_prefix)" > diff --git a/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_nodelet.launch b/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_nodelet.launch index c9f45084..05c542b3 100644 --- a/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_nodelet.launch +++ b/rubis_ws/src/vesc/vesc_driver/launch/vesc_driver_nodelet.launch @@ -21,7 +21,7 @@ + args="manager" launch-prefix="$(arg launch_prefix)">