diff --git a/.cspell.json b/.cspell.json index 2c51a351..66c0f12b 100644 --- a/.cspell.json +++ b/.cspell.json @@ -50,15 +50,18 @@ "gird" ], "words": [ + "accelmap", "ackermann", "adapi", "aichallenge", "astar", "autocompute", + "autodetermine", "automotiveaichallenge", "autoware", "awsim", "booars", + "brakemap", "buildtool", "colcon", "costmap", @@ -66,6 +69,7 @@ "cyclonedds", "dallara", "dcmake", + "decel", "distro", "downsample", "freespace", diff --git a/Dockerfile b/Dockerfile index 08609300..b8ab1752 100644 --- a/Dockerfile +++ b/Dockerfile @@ -11,8 +11,12 @@ RUN apt-get -y install ros-humble-rqt-graph # PATH="$PATH:/root/.local/bin" # PATH="/usr/local/cuda/bin:$PATH" ENV XDG_RUNTIME_DIR=/tmp/xdg -ENV ROS_LOCALHOST_ONLY=1 +ENV ROS_LOCALHOST_ONLY=0 ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +ENV ROS_DOMAIN_ID=71 +ENV CYCLONEDDS_URI=file:///opt/autoware/cyclonedds.xml + +COPY cyclonedds.xml /opt/autoware/cyclonedds.xml FROM common AS dev diff --git a/aichallenge/pkill_all.sh b/aichallenge/pkill_all.sh index e2c07d9f..646f10b2 100755 --- a/aichallenge/pkill_all.sh +++ b/aichallenge/pkill_all.sh @@ -1,3 +1,5 @@ +#!/bin/bash + pkill -9 AWSIM.x86_64 pkill -9 component_conta pkill -9 dummy_objects_p @@ -18,4 +20,4 @@ pkill -9 routing_adaptor pkill -9 rviz2 pkill -9 simple_pure_pur pkill -9 twist2accel -pkill -9 vehicle_velocit \ No newline at end of file +pkill -9 vehicle_velocit diff --git a/aichallenge/run_evaluation.bash b/aichallenge/run_evaluation.bash index 90d9c142..46fd6582 100755 --- a/aichallenge/run_evaluation.bash +++ b/aichallenge/run_evaluation.bash @@ -47,9 +47,33 @@ wmctrl -a "AWSIM" && wmctrl -r "AWSIM" -e 0,0,0,900,1043 ros2 service call /debug/service/capture_screen std_srvs/srv/Trigger >/dev/null sleep 1 +# Set initial pose +echo "Set initial pose" +ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{ + header: { + frame_id: 'map' + }, + pose: { + pose: { + position: { + x: 89633.29, + y: 43127.57, + z: 0.0 + }, + orientation: { + x: 0.0, + y: 0.0, + z: 0.8778, + w: 0.4788 + } + } + } +}" >/dev/null +sleep 1 + # Start driving and wait for the simulation to finish echo "Waiting for the simulation" -ros2 topic pub --once /control/control_mode_request_topic std_msgs/msg/Bool '{data: true}' >/dev/null +ros2 service call /control/control_mode_request autoware_auto_vehicle_msgs/srv/ControlModeCommand '{mode: 1}' >/dev/null wait $PID_AWSIM # Stop recording rviz2 diff --git a/aichallenge/workspace/run_aichallenge_launch_replay_rosbag.sh b/aichallenge/workspace/run_aichallenge_launch_replay_rosbag.sh new file mode 100644 index 00000000..d7e86d0e --- /dev/null +++ b/aichallenge/workspace/run_aichallenge_launch_replay_rosbag.sh @@ -0,0 +1,5 @@ +#!/usr/bin/bash + +source install/setup.bash + +ros2 launch aichallenge_system_launch aichallenge_system.launch.xml simulation:=false use_sim_time:=true run_rviz:=true diff --git a/aichallenge/workspace/run_aichallenge_launch_run_vehicle.sh b/aichallenge/workspace/run_aichallenge_launch_run_vehicle.sh new file mode 100644 index 00000000..2497e9e9 --- /dev/null +++ b/aichallenge/workspace/run_aichallenge_launch_run_vehicle.sh @@ -0,0 +1,5 @@ +#!/usr/bin/bash + +source install/setup.bash + +ros2 launch aichallenge_system_launch aichallenge_system.launch.xml simulation:=false use_sim_time:=false run_rviz:=false diff --git a/aichallenge/workspace/run_aichallenge_launch_simulation.sh b/aichallenge/workspace/run_aichallenge_launch_simulation.sh new file mode 100644 index 00000000..406f621a --- /dev/null +++ b/aichallenge/workspace/run_aichallenge_launch_simulation.sh @@ -0,0 +1,5 @@ +#!/usr/bin/bash + +source install/setup.bash + +ros2 launch aichallenge_system_launch aichallenge_system.launch.xml simulation:=true use_sim_time:=true run_rviz:=true diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/imu_corrector.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/imu_corrector.param.yaml deleted file mode 100644 index 3156598f..00000000 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/imu_corrector.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - angular_velocity_offset_x: 0.0 # [rad/s] - angular_velocity_offset_y: 0.0 # [rad/s] - angular_velocity_offset_z: 0.0 # [rad/s] - angular_velocity_stddev_xx: 0.03 # [rad/s] - angular_velocity_stddev_yy: 0.03 # [rad/s] - angular_velocity_stddev_zz: 0.03 # [rad/s] diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/map/lanelet2_map_loader.param.yaml similarity index 100% rename from aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml rename to aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/map/lanelet2_map_loader.param.yaml diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml new file mode 100644 index 00000000..a801eca3 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + map_frame: map + arrival_check_angle_deg: 45.0 + arrival_check_distance: 1.0 + arrival_check_duration: 1.0 + goal_angle_threshold_deg: 45.0 + enable_correct_goal_pose: false diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml new file mode 100644 index 00000000..eb670976 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/common/nearest_search.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + # ego + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml new file mode 100644 index 00000000..c14f59f6 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -0,0 +1,116 @@ +# see AvoidanceParameters description in avoidance_module_data.hpp for description. +/**: + ros__parameters: + avoidance: + resample_interval_for_planning: 0.3 # [m] + resample_interval_for_output: 4.0 # [m] + detection_area_right_expand_dist: 0.0 # [m] + detection_area_left_expand_dist: 1.0 # [m] + drivable_area_right_bound_offset: 0.0 # [m] + drivable_area_left_bound_offset: 0.0 # [m] + object_envelope_buffer: 0.3 # [m] + + # avoidance module common setting + enable_bound_clipping: false + enable_avoidance_over_same_direction: true + enable_avoidance_over_opposite_direction: true + enable_update_path_when_object_is_gone: false + enable_force_avoidance_for_stopped_vehicle: false + enable_safety_check: false + enable_yield_maneuver: false + disable_path_update: false + + # for debug + publish_debug_marker: false + print_debug_info: false + + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: false + pedestrian: false + + # For target object filtering + target_filtering: + # filtering moving objects + threshold_speed_object_is_stopped: 1.0 # [m/s] + threshold_time_object_is_moving: 1.0 # [s] + threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] + # detection range + object_check_force_avoidance_clearance: 30.0 # [m] + object_check_forward_distance: 150.0 # [m] + object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] + # filtering parking objects + threshold_distance_object_is_on_center: 1.0 # [m] + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + # lost object compensation + object_last_seen_threshold: 2.0 + + # For safety check + safety_check: + safety_check_backward_distance: 100.0 # [m] + safety_check_time_horizon: 10.0 # [s] + safety_check_idling_time: 1.5 # [s] + safety_check_accel_for_rss: 2.5 # [m/ss] + safety_check_hysteresis_factor: 2.0 # [-] + + # For avoidance maneuver + avoidance: + # avoidance lateral parameters + lateral: + lateral_collision_margin: 1.0 # [m] + lateral_collision_safety_buffer: 0.7 # [m] + lateral_passable_safety_buffer: 0.0 # [m] + road_shoulder_safety_margin: 0.3 # [m] + avoidance_execution_lateral_threshold: 0.499 + max_right_shift_length: 5.0 + max_left_shift_length: 5.0 + # avoidance distance parameters + longitudinal: + prepare_time: 2.0 # [s] + longitudinal_collision_safety_buffer: 0.0 # [m] + min_prepare_distance: 1.0 # [m] + min_avoidance_distance: 10.0 # [m] + min_nominal_avoidance_speed: 7.0 # [m/s] + min_sharp_avoidance_speed: 1.0 # [m/s] + + # For yield maneuver + yield: + yield_velocity: 2.78 # [m/s] + + # For stop maneuver + stop: + min_distance: 10.0 # [m] + max_distance: 20.0 # [m] + + constraints: + # vehicle slows down under longitudinal constraints + use_constraints_for_decel: false # [-] + + # lateral constraints + lateral: + nominal_lateral_jerk: 0.2 # [m/s3] + max_lateral_jerk: 1.0 # [m/s3] + + # longitudinal constraints + longitudinal: + nominal_deceleration: -1.0 # [m/ss] + nominal_jerk: 0.5 # [m/sss] + max_deceleration: -2.0 # [m/ss] + max_jerk: 1.0 + # For prevention of large acceleration while avoidance + min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] + max_avoidance_acceleration: 0.5 # [m/ss] + + target_velocity_matrix: + col_size: 2 + matrix: + [2.78, 13.9, # velocity [m/s] + 0.50, 1.00] # margin [m] diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml new file mode 100644 index 00000000..902a93ce --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -0,0 +1,58 @@ +/**: + ros__parameters: + # Static expansion + avoidance: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + lane_change: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + lane_following: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + pull_out: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + pull_over: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + side_shift: + drivable_area_right_bound_offset: 0.0 + drivable_area_left_bound_offset: 0.0 + drivable_area_types_to_skip: [road_border] + + # Dynamic expansion by projecting the ego footprint along the path + dynamic_expansion: + enabled: false + ego: + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the ego footprint + rear: 0.5 # [m] extra length to add to the rear of the ego footprint + left: 0.5 # [m] extra length to add to the left of the ego footprint + right: 0.5 # [m] extra length to add to the rear of the ego footprint + dynamic_objects: + avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects + extra_footprint_offset: + front: 0.5 # [m] extra length to add to the front of the dynamic object footprint + rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + left: 0.5 # [m] extra length to add to the left of the dynamic object footprint + right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint + expansion: + method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. + # 'lanelet': add lanelets overlapped by the ego footprints + # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area + max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + avoid_linestring: + types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area + - road_border + distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid + compensate: + enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction + extra_distance: 3.0 # [m] extra distance to add to the compensation diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml new file mode 100644 index 00000000..295769ac --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -0,0 +1,34 @@ +/**: + ros__parameters: + lane_change: + lane_change_prepare_duration: 4.0 # [s] + lane_changing_safety_check_duration: 8.0 # [s] + + minimum_lane_change_prepare_distance: 2.0 # [m] + minimum_lane_change_length: 16.5 # [m] + backward_length_buffer_for_end_of_lane: 3.0 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] + + lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_acc: 0.5 # [m/s2] + + minimum_lane_change_velocity: 2.78 # [m/s] + prediction_time_resolution: 0.5 # [s] + maximum_deceleration: 1.0 # [m/s2] + lane_change_sampling_num: 3 + + # collision check + enable_collision_check_at_prepare_phase: false + prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s] + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + + # abort + enable_cancel_lane_change: true + enable_abort_lane_change: false + + abort_delta_time: 3.0 # [s] + abort_max_lateral_jerk: 1000.0 # [m/s3] + + # debug + publish_debug_marker: false diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml new file mode 100644 index 00000000..b6a9574b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + lane_following: + lane_change_prepare_duration: 2.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml new file mode 100644 index 00000000..68000747 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml @@ -0,0 +1,31 @@ +/**: + ros__parameters: + pull_out: + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margin: 1.0 + collision_check_distance_from_end: 1.0 + # shift pull out + enable_shift_pull_out: true + shift_pull_out_velocity: 2.0 + pull_out_sampling_num: 4 + minimum_shift_pull_out_distance: 20.0 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 + # geometric pull out + enable_geometric_pull_out: true + divide_pull_out_path: false + geometric_pull_out_velocity: 1.0 + arc_path_interval: 1.0 + lane_departure_margin: 0.2 + backward_velocity: -1.0 + pull_out_max_steer_angle: 0.26 # 15deg + # search start pose backward + enable_back: true + search_priority: "efficient_path" # "efficient_path" or "short_back_distance" + max_back_distance: 30.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 + ignore_distance_from_lane_end: 15.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml new file mode 100644 index 00000000..ca1eb740 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml @@ -0,0 +1,95 @@ +/**: + ros__parameters: + pull_over: + request_length: 100.0 + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 2.0 # It must be greater than the state_machine's. + pull_over_velocity: 3.0 + pull_over_minimum_velocity: 1.38 + margin_from_boundary: 0.5 + decide_path_distance: 10.0 + maximum_deceleration: 0.5 + # goal research + enable_goal_research: true + search_priority: "efficient_path" # "efficient_path" or "close_goal" + forward_goal_search_length: 20.0 + backward_goal_search_length: 20.0 + goal_search_interval: 2.0 + longitudinal_margin: 3.0 + max_lateral_offset: 0.5 + lateral_offset_interval: 0.25 + ignore_distance_from_lane_start: 15.0 + # occupancy grid map + use_occupancy_grid: true + use_occupancy_grid_for_longitudinal_margin: false + occupancy_grid_collision_check_margin: 0.0 + theta_size: 360 + obstacle_threshold: 60 + # object recognition + use_object_recognition: true + object_recognition_collision_check_margin: 1.0 + # shift path + enable_shift_parking: true + pull_over_sampling_num: 4 + maximum_lateral_jerk: 2.0 + minimum_lateral_jerk: 0.5 + deceleration_interval: 15.0 + after_pull_over_straight_distance: 1.0 + # freespace parking + enable_freespace_parking: true + freespace_parking: + planning_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 + # parallel parking path + enable_arc_forward_parking: true + enable_arc_backward_parking: true + after_forward_parking_straight_distance: 2.0 + after_backward_parking_straight_distance: 2.0 + forward_parking_velocity: 1.38 + backward_parking_velocity: -1.38 + forward_parking_lane_departure_margin: 0.0 + backward_parking_lane_departure_margin: 0.0 + arc_path_interval: 1.0 + pull_over_max_steer_angle: 0.35 # 20deg + # hazard on when parked + hazard_on_threshold_distance: 1.0 + hazard_on_threshold_velocity: 0.5 + # check safety with dynamic objects. Not used now. + pull_over_duration: 2.0 + pull_over_prepare_duration: 4.0 + min_stop_distance: 5.0 + stop_time: 2.0 + hysteresis_buffer_distance: 2.0 + prediction_time_resolution: 0.5 + enable_collision_check_at_prepare_phase: false + use_predicted_path_outside_lanelet: false + use_all_predicted_path: false + # debug + print_debug_info: false diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml new file mode 100644 index 00000000..79044041 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + side_shift: + min_distance_to_start_shifting: 5.0 + time_to_start_shifting: 1.0 + shifting_lateral_jerk: 0.2 + min_shifting_distance: 5.0 + min_shifting_speed: 5.56 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/vehicle_velocity_converter.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/vehicle_velocity_converter.param.yaml new file mode 100644 index 00000000..649296ff --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/vehicle_velocity_converter.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + speed_scale_factor: 1.0 # [.] + velocity_stddev_xx: 0.2 # [m/s] + angular_velocity_stddev_zz: 0.1 # [rad/s] + frame_id: base_link diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv index 32e639ca..95879ae7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv @@ -1,7 +1,10 @@ -default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 -0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 -0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28 -0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03 -0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58 -0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1 -0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61 +default, 0.0 ,2.0 ,4.0 ,6.0 ,8.0 ,10.0 + 0.0, -0.3 ,-0.3 ,-0.3 ,-0.3 ,-0.3 ,-0.3 + 0.1, 0.0 ,0.0 ,0.0 ,0.0 ,0.0 ,0.0 + 0.2, 0.01 ,0.01 ,0.01 ,0.01 ,0.01 ,0.01 + 0.3, 0.05 ,0.05 ,0.05 ,0.05 ,0.05 ,0.05 + 0.4, 0.1 ,0.1 ,0.1 ,0.1 ,0.1 ,0.1 + 0.5, 0.175 ,0.175 ,0.175 ,0.175 ,0.175 ,0.175 + 0.6, 0.25 ,0.25 ,0.25 ,0.25 ,0.25 ,0.25 + 0.8, 0.7 ,0.7 ,0.7 ,0.7 ,0.7 ,0.7 + 1.0, 1.0 ,1.0 ,1.0 ,1.0 ,1.0 ,1.0 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/brake_map.csv b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/brake_map.csv index adf2c809..3a059f61 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/brake_map.csv +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/brake_map.csv @@ -1,10 +1,9 @@ -default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89 -0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5 -0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51 -0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96 -0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633 -0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106 -0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116 -0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126 -0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136 -0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955 + default, 0.0, 2.0, 4.0, 6.0, 8.0, 10.0 + 0.0, -0.3, -0.3, -0.3, -0.3, -0.3, -0.3 + 0.1, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5 + 0.2, -0.7, -0.7, -0.7, -0.7, -0.7, -0.7 + 0.3, -0.9, -0.9, -0.9, -0.9, -0.9, -0.9 + 0.4, -1.1, -1.1, -1.1, -1.1, -1.1, -1.1 + 0.5, -1.3, -1.3, -1.3, -1.3, -1.3, -1.3 + 0.6, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5 + 1.0, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5 diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml index 510f861c..a625ec50 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml @@ -1,7 +1,24 @@ - - + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml index ac984457..d671b0bb 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml @@ -2,20 +2,19 @@ - - + + - - + + - + - - + @@ -29,7 +28,7 @@ - + @@ -40,19 +39,21 @@ + - - - - - - + + + + + + - + + @@ -81,6 +82,8 @@ + + @@ -115,7 +118,7 @@ - + @@ -180,14 +183,14 @@ - - - - - - + + + + + + - + @@ -204,12 +207,17 @@ + + + + - - - + + + + @@ -227,7 +235,7 @@ - + @@ -236,7 +244,7 @@ - + diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/map/lanelet2_map.osm b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/map/lanelet2_map.osm index 0c9222a4..49147919 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/map/lanelet2_map.osm +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/map/lanelet2_map.osm @@ -1,5126 +1,3288 @@ - - - + + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - + + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - - - - - - - - - - - - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - + - - - - - - - - + - - + - - + - - + - - - - - - - - + - - + - - - - - - - - + - - - - - - - - - - - - - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - - - - - - - - - - - - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - - - - - - - + - - + - - + - - - - - - - - - - - - - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - + - - + - - - - + + + - - - - - - - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + - - - - - - - - + - - + - - - - - - - - + - - - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - + - - - - - - - - + - - - - - - - - + - - - - - - - - + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - - + + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - - - - - - - - - - - - - + + + - - + - - + - - - + + - - - + + - - - + + - - - + + - - - - - - - - - - - - - - - - + + + - - - - + + + - - - - + + + - - - - - - - - - - - - - - - + + - - - + + - - - - - - - - - - - - - - - - + + + - - - - + + + - - + - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - - - + + + - - - - + + + - - - - + + + - - + - - - - - - - - + - - - - - - - - - - + + + - - - - + + + - - - - + + + - - - - - - - - + - - - - - - - - + - - - - + + + - - - - + + + - - + - - - - + + + - - + - - - - + + + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - - - + + + - - + - - + - - + - - - - - - - - + - - - - + + + - - - - - - - - + - - - - - - - - - - - - - - - - - - - - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + - - - - + + + - - + + + + + + + + + + + + + + + + + + + + + + @@ -5133,7 +3295,6 @@ - @@ -5149,7 +3310,234 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -5183,7 +3571,6 @@ - @@ -5198,16 +3585,11 @@ - - - - - @@ -5220,30 +3602,20 @@ - - - - - - - - - - @@ -5251,177 +3623,11 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - + @@ -5437,15 +3643,10 @@ - - - - - @@ -5503,35 +3704,173 @@ - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - @@ -5550,17 +3889,11 @@ - - - - - - @@ -5589,6 +3922,12 @@ + + + + + + @@ -5604,16 +3943,11 @@ - - - - - @@ -5625,10 +3959,8 @@ - - @@ -5665,360 +3997,43 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + - + + + + + + + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/package.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/package.xml index 38c239a7..6798de33 100644 --- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/package.xml @@ -8,6 +8,38 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake + tier4_vehicle_launch + global_parameter_loader + goal_pose_setter + map_loader + map_tf_generator + imu_corrector + imu_gnss_poser + gyro_odometer + ekf_localizer + twist2accel + raw_vehicle_cmd_converter + robot_state_publisher + vehicle_velocity_converter + racing_kart_description + racing_kart_sensor_kit_description + + dummy_perception_publisher + mission_planner + behavior_path_planner + path_to_trajectory + default_ad_api + ad_api_adaptors + simple_pure_pursuit + + tier4_planning_rviz_plugin + tier4_localization_rviz_plugin + tier4_control_rviz_plugin + tier4_vehicle_rviz_plugin + multi_data_monitor + generic_type_utility + topic_tools ament_lint_auto autoware_lint_common diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/CMakeLists.txt index 417df8f1..9c4e6956 100644 --- a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/CMakeLists.txt @@ -17,4 +17,4 @@ ament_auto_add_executable(goal_pose_setter_node ament_auto_package( INSTALL_TO_SHARE config -) \ No newline at end of file +) diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/config/imu_corrector.param.yaml b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/config/imu_corrector.param.yaml index 3156598f..6e47ff8a 100644 --- a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/config/imu_corrector.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/config/imu_corrector.param.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: - angular_velocity_offset_x: 0.0 # [rad/s] - angular_velocity_offset_y: 0.0 # [rad/s] - angular_velocity_offset_z: 0.0 # [rad/s] - angular_velocity_stddev_xx: 0.03 # [rad/s] - angular_velocity_stddev_yy: 0.03 # [rad/s] - angular_velocity_stddev_zz: 0.03 # [rad/s] + angular_velocity_offset_x: 0.0 # [rad/s] + angular_velocity_offset_y: 0.0 # [rad/s] + angular_velocity_offset_z: -0.004 # [rad/s] + angular_velocity_stddev_xx: 0.03 # [rad/s] + angular_velocity_stddev_yy: 0.03 # [rad/s] + angular_velocity_stddev_zz: 0.03 # [rad/s] diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/CMakeLists.txt index 06bdc746..90925db7 100644 --- a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/CMakeLists.txt @@ -16,4 +16,4 @@ ament_auto_add_executable(imu_gnss_poser_node ament_auto_package( INSTALL_TO_SHARE -) \ No newline at end of file +) diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp index f48f9707..8cc0fbce 100644 --- a/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/imu_gnss_poser/src/imu_gnss_poser_node.cpp @@ -14,7 +14,7 @@ class ImuGnssPoser : public rclcpp::Node pub_initial_pose_3d_ = this->create_publisher("/localization/initial_pose3d", qos); sub_twist_ = this->create_subscription( "/localization/twist_with_covariance", qos, - std::bind(&ImuGnssPoser::twist_callback, this, std::placeholders::_1)); + std::bind(&ImuGnssPoser::twist_callback, this, std::placeholders::_1)); sub_gnss_ = this->create_subscription( "/sensing/gnss/pose_with_covariance", qos, std::bind(&ImuGnssPoser::gnss_callback, this, std::placeholders::_1)); @@ -26,16 +26,30 @@ class ImuGnssPoser : public rclcpp::Node private: void gnss_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - msg->pose.pose.orientation.x = imu_msg_.orientation.x; - msg->pose.pose.orientation.y = imu_msg_.orientation.y; - msg->pose.pose.orientation.z = imu_msg_.orientation.z; - msg->pose.pose.orientation.w = imu_msg_.orientation.w; + + // this covariance means orientation is not reliable msg->pose.covariance[7*0] = 0.1; msg->pose.covariance[7*1] = 0.1; msg->pose.covariance[7*2] = 0.1; - msg->pose.covariance[7*3] = 0.01; - msg->pose.covariance[7*4] = 0.01; - msg->pose.covariance[7*5] = 0.1; + msg->pose.covariance[7*3] = 100000.0; + msg->pose.covariance[7*4] = 100000.0; + msg->pose.covariance[7*5] = 100000.0; + + // insert imu if orientation is nan or empty + if (std::isnan(msg->pose.pose.orientation.x) || + std::isnan(msg->pose.pose.orientation.y) || + std::isnan(msg->pose.pose.orientation.z) || + std::isnan(msg->pose.pose.orientation.w) || + (msg->pose.pose.orientation.x == 0 && + msg->pose.pose.orientation.y == 0 && + msg->pose.pose.orientation.z == 0 && + msg->pose.pose.orientation.w == 0)) + { + msg->pose.pose.orientation.x = imu_msg_.orientation.x; + msg->pose.pose.orientation.y = imu_msg_.orientation.y; + msg->pose.pose.orientation.z = imu_msg_.orientation.z; + msg->pose.pose.orientation.w = imu_msg_.orientation.w; + } pub_pose_->publish(*msg); if (!is_ekf_initialized_) pub_initial_pose_3d_->publish(*msg); @@ -45,7 +59,7 @@ class ImuGnssPoser : public rclcpp::Node imu_msg_ = *msg; } - void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr) { is_ekf_initialized_ = true; } diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/CMakeLists.txt index 5a9cfe39..01e1d6f3 100644 --- a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/CMakeLists.txt @@ -14,4 +14,4 @@ ament_auto_add_executable(path_to_trajectory_node src/path_to_trajectory.cpp ) -ament_auto_package() \ No newline at end of file +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp index 8658c0e6..c24836f8 100644 --- a/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp +++ b/aichallenge/workspace/src/aichallenge_submit/path_to_trajectory/include/path_to_trajectory/path_to_trajectory.hpp @@ -34,4 +34,4 @@ class PathToTrajectory : public rclcpp::Node { rclcpp::Publisher::SharedPtr pub_; }; -#endif // PATH_TO_TRAJECTORY__PATH_TO_TRAJECTORY_HPP_ \ No newline at end of file +#endif // PATH_TO_TRAJECTORY__PATH_TO_TRAJECTORY_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/CMakeLists.txt index 1876bbe6..ab052753 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/CMakeLists.txt @@ -12,4 +12,4 @@ ament_auto_package(INSTALL_TO_SHARE mesh urdf config -) \ No newline at end of file +) diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/config/vehicle_info.param.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/config/vehicle_info.param.yaml index 16fc2038..01d1b9e8 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/config/vehicle_info.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/config/vehicle_info.param.yaml @@ -9,4 +9,4 @@ left_overhang: 0.09 # between rear left wheel center and vehicle left (the half of the rear wheel width) right_overhang: 0.09 # between rear right wheel center and vehicle right (the half of the rear wheel width) vehicle_height: 2.2 # vehicle height + sensor_kit height - max_steer_angle: 0.64 # [rad] \ No newline at end of file + max_steer_angle: 0.64 # [rad] diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/mesh/square.dae b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/mesh/square.dae index d2e5996f..26677ed8 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/mesh/square.dae +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/mesh/square.dae @@ -1254,4 +1254,4 @@ - \ No newline at end of file + diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/package.xml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/package.xml index 70c9c545..241d1988 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/package.xml @@ -13,4 +13,4 @@ ament_cmake - \ No newline at end of file + diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/urdf/vehicle.xacro b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/urdf/vehicle.xacro index 4797b087..8636311a 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/urdf/vehicle.xacro +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_description/urdf/vehicle.xacro @@ -14,4 +14,4 @@ - \ No newline at end of file + diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/CMakeLists.txt index de64416f..3253ebd0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/CMakeLists.txt +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/CMakeLists.txt @@ -8,4 +8,4 @@ ament_auto_find_build_dependencies() ament_auto_package(INSTALL_TO_SHARE urdf config -) \ No newline at end of file +) diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/awsim_sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/awsim_sensor_kit_calibration.yaml new file mode 100644 index 00000000..f57e1627 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/awsim_sensor_kit_calibration.yaml @@ -0,0 +1,15 @@ +sensor_kit_base_link: + gnss_link: + x: -0.26 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + imu_link: + x: 0.85 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: -1.5707963268 diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml index 1778c130..83d47a9d 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/config/sensor_kit_calibration.yaml @@ -1,15 +1,22 @@ sensor_kit_base_link: gnss_link: - x: 0.0 + x: -0.26 y: 0.0 z: 0.0 roll: 0.0 pitch: 0.0 yaw: 0.0 + gnss_link2: + x: 1.46 + y: 0.0 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 imu_link: - x: 0.0 + x: 0.85 y: 0.0 z: 0.0 roll: 0.0 pitch: 0.0 - yaw: 0.0 + yaw: -1.5707963268 diff --git a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro index 4f2ca4fa..bdb94439 100644 --- a/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro +++ b/aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro @@ -5,6 +5,7 @@ + @@ -19,7 +20,10 @@ - + + + + #include #include +#include #include #include #include @@ -18,6 +19,7 @@ using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PointStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; using visualization_msgs::msg::Marker; @@ -32,6 +34,8 @@ class SimplePurePursuit : public rclcpp::Node { // publishers rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_raw_cmd_; + rclcpp::Publisher::SharedPtr mkr_cmd_; // timer @@ -50,6 +54,7 @@ class SimplePurePursuit : public rclcpp::Node { double speed_proportional_gain_; bool use_external_target_vel_; double external_target_vel_; + const double steering_tire_angle_gain_; OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; @@ -61,4 +66,4 @@ class SimplePurePursuit : public rclcpp::Node { } // namespace simple_pure_pursuit -#endif // SIMPLE_PURE_PURSUIT_HPP_ \ No newline at end of file +#endif // SIMPLE_PURE_PURSUIT_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/package.xml b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/package.xml index 704c4765..34447be0 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/package.xml +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/package.xml @@ -17,4 +17,4 @@ ament_cmake - \ No newline at end of file + diff --git a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp index 736185e1..b4406f6a 100644 --- a/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -22,9 +22,11 @@ SimplePurePursuit::SimplePurePursuit() lookahead_min_distance_(declare_parameter("lookahead_min_distance", 1.0)), speed_proportional_gain_(declare_parameter("speed_proportional_gain", 1.0)), use_external_target_vel_(declare_parameter("use_external_target_vel", false)), - external_target_vel_(declare_parameter("external_target_vel", 0.0)) + external_target_vel_(declare_parameter("external_target_vel", 0.0)), + steering_tire_angle_gain_(declare_parameter("steering_tire_angle_gain", 1.0)) { pub_cmd_ = create_publisher("output/control_cmd", 1); + pub_raw_cmd_ = create_publisher("output/raw_control_cmd", 1); mkr_cmd_ = create_publisher("debug/pursuit_lookahead", 1); sub_kinematics_ = create_subscription( @@ -68,7 +70,7 @@ void SimplePurePursuit::onTimer() if ( (closet_traj_point_idx == trajectory_->points.size() - 1) || - (trajectory_->points.size() <= 5)) { + (trajectory_->points.size() <= 2)) { cmd.longitudinal.speed = 0.0; cmd.longitudinal.acceleration = -10.0; RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal"); @@ -134,9 +136,11 @@ void SimplePurePursuit::onTimer() double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) - tf2::getYaw(odometry_->pose.pose.orientation); cmd.lateral.steering_tire_angle = - std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance); + steering_tire_angle_gain_ * std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance); } pub_cmd_->publish(cmd); + cmd.lateral.steering_tire_angle /= steering_tire_angle_gain_; + pub_raw_cmd_->publish(cmd); } bool SimplePurePursuit::subscribeMessageAvailable() diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt new file mode 100644 index 00000000..4cb36878 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(aichallenge_awsim_adapter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/actuation_cmd_converter.cpp + src/sensor_converter.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ActuationCmdConverter" + EXECUTABLE actuation_cmd_converter +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "SensorConverter" + EXECUTABLE sensor_converter +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/config/sensor_converter.param.yaml b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/config/sensor_converter.param.yaml new file mode 100644 index 00000000..a8683735 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/config/sensor_converter.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + gnss_pose_delay: 500 # [ms] + gnss_pose_cov_delay: 500 # [ms] + gnss_pose_mean: 0.0 + gnss_pose_stddev: 0.005 + gnss_pose_cov_mean: 0.0 + gnss_pose_cov_stddev: 0.007 + + imu_acc_mean: 0.0 + imu_acc_stddev: 0.001 + imu_ang_mean: 0.0 + imu_ang_stddev: 0.0025 + imu_ori_mean: 0.0 + imu_ori_stddev: 0.001 + + steering_angle_mean: 0.0 + steering_angle_stddev: 0.0000075 diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml new file mode 100644 index 00000000..090a0514 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml new file mode 100644 index 00000000..fa568759 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml @@ -0,0 +1,28 @@ + + + + aichallenge_awsim_adapter + 0.1.0 + The aichallenge_awsim_adapter package + Takagi Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + tier4_vehicle_msgs + raw_vehicle_cmd_converter + rclcpp + rclcpp_components + sensor_msgs + geometry_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.cpp b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.cpp new file mode 100644 index 00000000..fc911d3d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.cpp @@ -0,0 +1,92 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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 "actuation_cmd_converter.hpp" + +ActuationCmdConverter::ActuationCmdConverter(const rclcpp::NodeOptions & node_options) +: Node("actuation_cmd_converter", node_options) +{ + using std::placeholders::_1; + + // Parameters + const std::string csv_path_accel_map = declare_parameter("csv_path_accel_map"); + const std::string csv_path_brake_map = declare_parameter("csv_path_brake_map"); + + // Subscriptions + sub_actuation_ = create_subscription( + "/control/command/actuation_cmd", 1, std::bind(&ActuationCmdConverter::on_actuation_cmd, this, _1)); + sub_gear_ = create_subscription( + "/vehicle/status/gear_status", 1, std::bind(&ActuationCmdConverter::on_gear_report, this, _1)); + sub_velocity_ = create_subscription( + "/vehicle/status/velocity_status", 1, std::bind(&ActuationCmdConverter::on_velocity_report, this, _1)); + + // Publishers + pub_ackermann_ = create_publisher("/awsim/control_cmd", 1); + + // Load accel/brake map + if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { + RCLCPP_ERROR(get_logger(), "Cannot read accelmap. csv path = %s. stop calculation.", csv_path_accel_map.c_str()); + throw std::runtime_error("Cannot read accelmap."); + } + if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { + RCLCPP_ERROR(get_logger(), "Cannot read brakemap. csv path = %s. stop calculation.", csv_path_brake_map.c_str()); + throw std::runtime_error("Cannot read brakemap."); + } +} + +void ActuationCmdConverter::on_gear_report(const GearReport::ConstSharedPtr msg) +{ + gear_report_ = msg; +} + +void ActuationCmdConverter::on_velocity_report(const VelocityReport::ConstSharedPtr msg) +{ + velocity_report_ = msg; +} + +void ActuationCmdConverter::on_actuation_cmd(const ActuationCommandStamped::ConstSharedPtr msg) +{ + // Wait for input data + if (!gear_report_ || !velocity_report_) { + return; + } + + const double velocity = std::abs(velocity_report_->longitudinal_velocity); + const double acceleration = get_acceleration(*msg, velocity); + + // Publish ControlCommand + constexpr float nan = std::numeric_limits::quiet_NaN(); + AckermannControlCommand output; + output.stamp = msg->header.stamp; + output.lateral.steering_tire_angle = static_cast(msg->actuation.steer_cmd); + output.lateral.steering_tire_rotation_rate = nan; + output.longitudinal.speed = nan; + output.longitudinal.acceleration = static_cast(acceleration); + pub_ackermann_->publish(output); +} + +double ActuationCmdConverter::get_acceleration(const ActuationCommandStamped & cmd, const double velocity) +{ + const double desired_pedal = cmd.actuation.accel_cmd - cmd.actuation.brake_cmd; + double ref_acceleration = 0.0; + if (desired_pedal > 0.0) { + accel_map_.getAcceleration(+desired_pedal, velocity, ref_acceleration); + } else { + brake_map_.getAcceleration(-desired_pedal, velocity, ref_acceleration); + } + return ref_acceleration; +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ActuationCmdConverter) diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.hpp b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.hpp new file mode 100644 index 00000000..06e0f5e7 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.hpp @@ -0,0 +1,55 @@ +// Copyright 2020 Tier IV, Inc. +// +// 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. + +#ifndef AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ +#define AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include + +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_vehicle_msgs::msg::GearReport; +using autoware_auto_vehicle_msgs::msg::VelocityReport; +using tier4_vehicle_msgs::msg::ActuationCommandStamped; + +class ActuationCmdConverter : public rclcpp::Node +{ +public: + explicit ActuationCmdConverter(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Subscription::SharedPtr sub_actuation_; + rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Publisher::SharedPtr pub_ackermann_; + + void on_actuation_cmd(const ActuationCommandStamped::ConstSharedPtr msg); + void on_gear_report(const GearReport::ConstSharedPtr msg); + void on_velocity_report(const VelocityReport::ConstSharedPtr msg); + + double get_acceleration(const ActuationCommandStamped & cmd, const double velocity); + raw_vehicle_cmd_converter::AccelMap accel_map_; + raw_vehicle_cmd_converter::BrakeMap brake_map_; + GearReport::ConstSharedPtr gear_report_; + VelocityReport::ConstSharedPtr velocity_report_; +}; + +#endif // AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.cpp b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.cpp new file mode 100644 index 00000000..526e3c6e --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.cpp @@ -0,0 +1,134 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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 "sensor_converter.hpp" + +SensorConverter::SensorConverter(const rclcpp::NodeOptions & node_options) +: Node("sensor_converter", node_options) +{ + using std::placeholders::_1; + + // Parameters + gnss_pose_delay_ = declare_parameter("gnss_pose_delay"); + gnss_pose_cov_delay_ = declare_parameter("gnss_pose_cov_delay"); + gnss_pose_mean_ = declare_parameter("gnss_pose_mean"); + gnss_pose_stddev_ = declare_parameter("gnss_pose_stddev"); + gnss_pose_cov_mean_ = declare_parameter("gnss_pose_cov_mean"); + gnss_pose_cov_stddev_ = declare_parameter("gnss_pose_cov_stddev"); + imu_acc_mean_ = declare_parameter("imu_acc_mean"); + imu_acc_stddev_ = declare_parameter("imu_acc_stddev"); + imu_ang_mean_ = declare_parameter("imu_ang_mean"); + imu_ang_stddev_ = declare_parameter("imu_ang_stddev"); + imu_ori_mean_ = declare_parameter("imu_ori_mean"); + imu_ori_stddev_ = declare_parameter("imu_ori_stddev"); + steering_angle_mean_ = declare_parameter("steering_angle_mean"); + steering_angle_stddev_ = declare_parameter("steering_angle_stddev"); + + + // Subscriptions + sub_gnss_pose_ = create_subscription( + "/awsim/gnss/pose", 1, std::bind(&SensorConverter::on_gnss_pose, this, _1)); + sub_gnss_pose_cov_ = create_subscription( + "/awsim/gnss/pose_with_covariance", 1, std::bind(&SensorConverter::on_gnss_pose_cov, this, _1)); + sub_imu_ = create_subscription( + "/awsim/imu", 1, std::bind(&SensorConverter::on_imu, this, _1)); + sub_steering_report_ = create_subscription( + "/awsim/steering_status", 1, std::bind(&SensorConverter::on_steering_report, this, _1)); + + // Publishers + pub_gnss_pose_ = create_publisher("/sensing/gnss/pose", 1); + pub_gnss_pose_cov_ = create_publisher("/sensing/gnss/pose_with_covariance", 1); + pub_imu_ = create_publisher("/sensing/imu/imu_raw", 1); + pub_steering_report_ = create_publisher("/vehicle/status/steering_status", 1); + + std::random_device rd; + generator_ = std::mt19937(rd()); + pose_distribution_ = std::normal_distribution(gnss_pose_mean_, gnss_pose_stddev_); + pose_cov_distribution_ = std::normal_distribution(gnss_pose_mean_, gnss_pose_stddev_); + imu_acc_distribution_ = std::normal_distribution(imu_acc_mean_, imu_acc_stddev_); + imu_ang_distribution_ = std::normal_distribution(imu_ang_mean_, imu_ang_stddev_); + imu_ori_distribution_ = std::normal_distribution(imu_ori_mean_, imu_ori_stddev_); + steering_angle_distribution_ = std::normal_distribution(steering_angle_mean_, steering_angle_stddev_); +} + +void SensorConverter::on_gnss_pose(const PoseStamped::ConstSharedPtr msg) +{ + auto process_and_publish_gnss = [this, msg]() { + rclcpp::sleep_for(std::chrono::milliseconds(gnss_pose_delay_)); + + auto pose = std::make_shared(*msg); + pose->header.stamp = now(); + pose->pose.position.x += pose_distribution_(generator_); + pose->pose.position.y += pose_distribution_(generator_); + pose->pose.position.z += pose_distribution_(generator_); + pose->pose.orientation.x += pose_distribution_(generator_); + pose->pose.orientation.y += pose_distribution_(generator_); + pose->pose.orientation.z += pose_distribution_(generator_); + pose->pose.orientation.w += pose_distribution_(generator_); + + pub_gnss_pose_->publish(*pose); + }; + + std::thread processing_thread(process_and_publish_gnss); + processing_thread.detach(); +} + + +void SensorConverter::on_gnss_pose_cov(const PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + auto process_and_publish_gnss_cov = [this, msg]() { + rclcpp::sleep_for(std::chrono::milliseconds(gnss_pose_cov_delay_)); + + auto pose_cov = std::make_shared(*msg); + pose_cov->header.stamp = now(); + pose_cov->pose.pose.position.x += pose_cov_distribution_(generator_); + pose_cov->pose.pose.position.y += pose_cov_distribution_(generator_); + pose_cov->pose.pose.position.z += pose_cov_distribution_(generator_); + pose_cov->pose.pose.orientation.x += pose_cov_distribution_(generator_); + pose_cov->pose.pose.orientation.y += pose_cov_distribution_(generator_); + pose_cov->pose.pose.orientation.z += pose_cov_distribution_(generator_); + pose_cov->pose.pose.orientation.w += pose_cov_distribution_(generator_); + + pub_gnss_pose_cov_->publish(*pose_cov); + }; + + std::thread processing_thread(process_and_publish_gnss_cov); + processing_thread.detach(); +} + +void SensorConverter::on_imu(const Imu::ConstSharedPtr msg) +{ + imu_ = std::make_shared(*msg); + imu_ -> orientation.x += imu_ori_distribution_(generator_); + imu_ -> orientation.y += imu_ori_distribution_(generator_); + imu_ -> orientation.z += imu_ori_distribution_(generator_); + imu_ -> orientation.w += imu_ori_distribution_(generator_); + imu_ -> angular_velocity.x += imu_ang_distribution_(generator_); + imu_ -> angular_velocity.y += imu_ang_distribution_(generator_); + imu_ -> angular_velocity.z += imu_ang_distribution_(generator_); + imu_ -> linear_acceleration.x += imu_acc_distribution_(generator_); + imu_ -> linear_acceleration.y += imu_acc_distribution_(generator_); + imu_ -> linear_acceleration.z += imu_acc_distribution_(generator_); + pub_imu_->publish(*imu_); +} + +void SensorConverter::on_steering_report(const SteeringReport::ConstSharedPtr msg) +{ + steering_report_ = std::make_shared(*msg); + steering_report_->steering_tire_angle += steering_angle_distribution_(generator_); + pub_steering_report_->publish(*steering_report_); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(SensorConverter) diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.hpp b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.hpp new file mode 100644 index 00000000..a029189b --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.hpp @@ -0,0 +1,81 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. + +#ifndef AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ +#define AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ + + +#include +#include + +#include +#include +#include +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" + +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::PoseWithCovarianceStamped; +using sensor_msgs::msg::Imu; +using autoware_auto_vehicle_msgs::msg::SteeringReport; + +class SensorConverter : public rclcpp::Node +{ +public: + explicit SensorConverter(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Subscription::SharedPtr sub_gnss_pose_; + rclcpp::Subscription::SharedPtr sub_gnss_pose_cov_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_steering_report_; + rclcpp::Publisher::SharedPtr pub_gnss_pose_; + rclcpp::Publisher::SharedPtr pub_imu_; + rclcpp::Publisher::SharedPtr pub_gnss_pose_cov_; + rclcpp::Publisher::SharedPtr pub_steering_report_; + + + void on_gnss_pose(const PoseStamped::ConstSharedPtr msg); + void on_gnss_pose_cov(const PoseWithCovarianceStamped::ConstSharedPtr msg); + void on_imu(const Imu::ConstSharedPtr msg); + void on_steering_report(const SteeringReport::ConstSharedPtr msg); + + PoseStamped::SharedPtr pose_; + PoseWithCovarianceStamped::SharedPtr pose_cov_; + Imu::SharedPtr imu_; + SteeringReport::SharedPtr steering_report_; + int gnss_pose_delay_; + int gnss_pose_cov_delay_; + + std::mt19937 generator_; + std::normal_distribution pose_distribution_; + std::normal_distribution pose_cov_distribution_; + std::normal_distribution imu_acc_distribution_; + std::normal_distribution imu_ang_distribution_; + std::normal_distribution imu_ori_distribution_; + std::normal_distribution steering_angle_distribution_; + double gnss_pose_mean_; + double gnss_pose_stddev_; + double gnss_pose_cov_mean_; + double gnss_pose_cov_stddev_; + double imu_acc_mean_; + double imu_acc_stddev_; + double imu_ang_mean_; + double imu_ang_stddev_; + double imu_ori_mean_; + double imu_ori_stddev_; + double steering_angle_mean_; + double steering_angle_stddev_; +}; + +#endif // AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_ diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.png b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.png new file mode 100644 index 00000000..cf2d1e8e Binary files /dev/null and b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.png differ diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz new file mode 100644 index 00000000..bc2090e8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/debug_sensing.rviz @@ -0,0 +1,1987 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /System1/TF1 + - /System1/Grid1 + - /Map1 + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Sensing1/GNSS1/PoseWithCovariance1 + - /Sensing1/Odometry1/Shape1 + - /Sensing1/BaseLink1 + - /Sensing1/GNSSLink1 + - /Sensing1/Sensorkit1 + - /Localization1 + - /Localization1/EKF1/PoseHistory1 + - /Planning1 + - /Planning1/ScenarioPlanning1/ScenarioTrajectory1 + - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1 + - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Velocity1 + - /Planning1/ScenarioPlanning1/LaneDriving1 + - /Control1 + - /Control1/PointStamped1 + Splitter Ratio: 0.557669460773468 + Tree Height: 331 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel + Name: InitialPoseButtonPanel + - Class: AutowareScreenCapturePanel + Name: AutowareScreenCapturePanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + gnss_link: + Value: true + imu_link: + Value: true + map: + Value: true + sensor_kit_base_link: + Value: true + viewer: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + base_link: + sensor_kit_base_link: + gnss_link: + {} + imu_link: + {} + viewer: + {} + Update Interval: 0 + Value: true + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/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_common/Group + Displays: + - Class: rviz_plugins/SteeringAngle + Enabled: true + Left: 50 + Length: 100 + Name: SteeringAngle + Scale: 3 + Text Color: 25; 255; 240 + Top: 30 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/steering_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Class: rviz_plugins/ConsoleMeter + Enabled: true + Left: 200 + Length: 100 + Name: ConsoleMeter + Text Color: 25; 255; 240 + Top: 35 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Alpha: 0.9990000128746033 + Class: rviz_plugins/VelocityHistory + Color Border Vel Max: 3 + Constant Color: + Color: 255; 255; 255 + Value: true + Enabled: true + Name: VelocityHistory + Scale: 0.30000001192092896 + Timeout: 10 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + 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 + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gnss_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_kit_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: VehicleModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.01 + Min Wave Alpha: 0.01 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Class: rviz_plugins/MaxVelocity + Enabled: true + Left: 635 + Length: 102 + Name: MaxVelocity + Text Color: 255; 255; 255 + Top: 299 + Topic: /planning/scenario_planning/current_max_velocity + Value: true + Value Scale: 0.25 + - Class: rviz_plugins/TurnSignal + Enabled: false + Height: 273 + Left: 209 + Name: TurnSignal + Top: 373 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/turn_indicators_status + Value: true + Width: 546 + Enabled: true + Name: Vehicle + Enabled: true + Name: System + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + center_lane_line: false + center_line_arrows: false + lane_start_bound: false + lanelet_id: false + left_lane_bound: true + right_lane_bound: true + road_lanelets: false + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.9990000128746033 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: MeasurementRange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/crop_box_filter/crop_box_polygon + Value: false + Enabled: false + Name: LiDAR + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 233; 185; 110 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.20000000298023224 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PoseWithCovariance + Shaft Length: 1.5 + Shaft Radius: 0.20000000298023224 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + Enabled: true + Name: GNSS + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 4 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/kinematic_state + Value: true + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 1 + Name: GNSSBaseLink + Radius: 0.10000000149011612 + Reference Frame: gnss_base_link + Value: false + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.20000000298023224 + Name: BaseLink + Radius: 0.30000001192092896 + Reference Frame: base_link + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: IMUAxes + Radius: 0.10000000149011612 + Reference Frame: imu_link + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: GNSSLink + Radius: 0.10000000149011612 + Reference Frame: gnss_link + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.4000000059604645 + Name: Sensorkit + Radius: 0.20000000298023224 + Reference Frame: sensor_kit_base_link + Value: true + Enabled: true + Name: Sensing + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 170; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovInitial + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/initial_pose_with_covariance + Value: true + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovAligned + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: false + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Initial + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /localization/util/downsample/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Aligned + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MonteCarloInitialPose + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/monte_carlo_initial_pose_marker + Value: false + Enabled: false + Name: GNSS + - Class: rviz_common/Group + Displays: + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 200 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/kinematic_state + Value: true + Enabled: true + Name: EKF + Enabled: true + Name: Localization + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: RouteArea + Namespaces: + goal_lanelets: true + lane_start_bound: false + left_lane_bound: false + right_lane_bound: false + route_lanelets: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/route_marker + Value: true + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.5 + Name: GoalPose + Shaft Length: 3 + Shaft Radius: 0.20000000298023224 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/echo_back_goal_pose + Value: true + Enabled: true + Name: MissionPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: ScenarioTrajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/trajectory + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 0.10000000149011612 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: Path + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/path + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_LaneChange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/lane_change + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_ExternalRequestLaneChangeRight + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/ext_request_lane_change_right + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_ExternalRequestLaneChangeLeft + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/ext_request_lane_change_left + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_Avoidance + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/avoidance + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_PullOut + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/pull_out + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_PullOver + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/pull_over + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Bound + Namespaces: + left_bound: true + right_bound: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (BlindSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Crosswalk) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Walkway) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/walkway + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DetectionArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Intersection) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (MergeFromPrivateArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (NoStoppingArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OcclusionSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (StopLine) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (TrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (VirtualTrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (RunOut) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SpeedBump) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Arrow + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Blind Spot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: VirtualTrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: StopLine + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: OcclusionSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: RunOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Avoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneChange + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneFollowing + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SideShift + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SpeedBump + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump + Value: false + Enabled: false + Name: DebugMarker + Enabled: true + Name: BehaviorPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/trajectory + Value: false + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleStop) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SurroundObstacle) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleCruise) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SurroundObstacleCheck + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Footprint + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 239; 41; 41 + Enabled: false + Name: FootprintOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 10; 21; 255 + Enabled: false + Name: FootprintRecoverOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset + Value: false + Enabled: false + Name: SurroundObstacleChecker + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: CruiseVirtualWall + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: DebugMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker + Value: true + Enabled: false + Name: ObstacleCruise + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: false + Enabled: false + Name: DebugMarker + Enabled: true + Name: MotionPlanning + Enabled: true + Name: LaneDriving + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates + Use Timestamp: false + Value: false + - Alpha: 0.9990000128746033 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PartialPoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array + Value: true + - Alpha: 0.9990000128746033 + Arrow Length: 0.5 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 255 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array + Value: true + Enabled: true + Name: Parking + - Class: rviz_plugins/PoseWithUuidStamped + Enabled: true + Length: 1.5 + Name: ModifiedGoal + Radius: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/modified_goal + UUID: + Scale: 0.30000001192092896 + Value: false + Value: true + Enabled: true + Name: ScenarioPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: PlanningErrorMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker + Value: true + Enabled: false + Name: Diagnostic + Enabled: true + Name: Planning + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 1 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/debug/lookahead_point + Value: true + Enabled: true + Name: Control + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /aichallenge/pitstop/area_marker + Value: true + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 237; 51; 59 + 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: 100 + Reference Frame: viewer + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 1 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /simple_pure_pursuit_node/debug/markers + Value: true + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Class: rviz_plugins/PedestrianInitialPoseTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/CarInitialPoseTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Theta std deviation: 0.0872664600610733 + Velocity: 3 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/BusInitialPoseTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/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: 41.36642837524414 + Target Frame: base_link + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + AutowareScreenCapturePanel: + collapsed: false + Displays: + collapsed: false + Height: 1074 + Hide Left Dock: false + Hide Right Dock: false + InitialPoseButtonPanel: + collapsed: false + MultiDataMonitor: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 2870 + Y: 2187 diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml index 9de12220..03c8124a 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml @@ -1,28 +1,45 @@ - - + - + - - + + + + + + + + - + + - - + + + + + + + + + + + + + + - + - - - - + + + diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml new file mode 100644 index 00000000..14889df6 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/real.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/real.launch.xml new file mode 100644 index 00000000..0ebf88f8 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/real.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/package.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/package.xml index 67a46d20..56c191e1 100644 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/package.xml +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/package.xml @@ -12,6 +12,7 @@ aichallenge_submit_launch std_msgs visualization_msgs + topic_tools ament_lint_auto autoware_lint_common diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py index 97de5d17..54108d77 100755 --- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py +++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py @@ -9,7 +9,7 @@ class ControlModeAdapterNode(rclpy.node.Node): def __init__(self): super().__init__("control_mode_adapter") self.sub = self.create_service(ControlModeCommand, "/control/control_mode_request", self.callback) - self.pub = self.create_publisher(Bool, "/control/control_mode_request_topic", 1) + self.pub = self.create_publisher(Bool, "/awsim/control_mode_request_topic", 1) def callback(self, req, res): msg = Bool() @@ -32,4 +32,7 @@ def main(args=None): rclpy.shutdown() if __name__ == "__main__": - main() + try: + main() + except KeyboardInterrupt: + pass diff --git a/cyclonedds.xml b/cyclonedds.xml new file mode 100644 index 00000000..37d2ba05 --- /dev/null +++ b/cyclonedds.xml @@ -0,0 +1,18 @@ + + + + + + + + default + 65500B + + + + + 500kB + + + +