Skip to content

Commit

Permalink
Update vehicle configuration parameters for CDAR VRU UC testing (#326)
Browse files Browse the repository at this point in the history
<!-- Thanks for the contribution, this is awesome. -->

# PR Details
## Description
Cherry Picking shared from
https://github.com/usdot-fhwa-stol/carma-config/tree/spike-smoke-test
branch. This change set is meant to be related to Carma Platform
configurations.

**Notable differences to smoke testing branch**
- Removed duplicate VehicleConfigParams.yaml in
`ail_vru_uc1_scenario/cdasim_config/carma`.
<!--- Describe your changes in detail -->

## Related Issue

<!--- This project only accepts pull requests related to open issues -->
<!--- If suggesting a new feature or change, please discuss it in an
issue first -->
<!--- If fixing a bug, there should be an issue describing it with steps
to reproduce -->
<!--- Please link to the issue here: -->

## Motivation and Context
CDAR-593
<!--- Why is this change required? What problem does it solve? -->

## How Has This Been Tested?

<!--- Please describe in detail how you tested your changes. -->
<!--- Include details of your testing environment, and the tests you ran
to -->
<!--- see how your change affects other areas of the code, etc. -->

## Types of changes

<!--- What types of changes does your code introduce? Put an `x` in all
the boxes that apply: -->

- [ ] Defect fix (non-breaking change that fixes an issue)
- [x] New feature (non-breaking change that adds functionality)
- [ ] Breaking change (fix or feature that cause existing functionality
to change)

## Checklist:

<!--- Go over all the following points, and put an `x` in all the boxes
that apply. -->
<!--- If you're unsure about any of these, don't hesitate to ask. We're
here to help! -->

- [ ] I have added any new packages to the sonar-scanner.properties file
- [ ] My change requires a change to the documentation.
- [ ] I have updated the documentation accordingly.
- [x] I have read the **CONTRIBUTING** document.
[CARMA Contributing
Guide](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md)
- [ ] I have added tests to cover my changes.
- [ ] All new and existing tests passed.
  • Loading branch information
paulbourelly999 committed Mar 15, 2024
1 parent 97e133d commit 109136d
Show file tree
Hide file tree
Showing 7 changed files with 317 additions and 36 deletions.
2 changes: 1 addition & 1 deletion ail_vru_uc1_scenario/VehicleConfigParams.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ enable_object_avoidance: true
# Acceleration limit
# Value type: Desired
# Units: m/s^2
vehicle_acceleration_limit: 2.0
vehicle_acceleration_limit: 3.0

# Deceleration limit
# Value type: Desired
Expand Down
124 changes: 93 additions & 31 deletions ail_vru_uc1_scenario/bridge.yml
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,14 @@
# NOTE: A custom dynamic_bridge is used in CARMA Platform which will automatically connect topics and services, but
# this file allows for certain topics to be pre-configured with specified QoS settings.
topics:
- topic: /guidance/plugins/debug/trajectory_planning
type: carma_debug_ros2_msgs/msg/TrajectoryCurvatureSpeeds
queue_size: 1
- topic: /guidance/trajectory_visualizer
type: visualization_msgs/msg/MarkerArray
queue_size: 1
-
topic: /environment/lanelet2_map_viz

- topic: /environment/lanelet2_map_viz
type: visualization_msgs/msg/MarkerArray
queue_size: 1
direction: 2to1
Expand All @@ -16,85 +19,144 @@ topics:
depth: 1
reliability: reliable
durability: transient_local
-
topic: /localization/points_map

- topic: /localization/points_map
type: sensor_msgs/msg/PointCloud2
queue_size: 1
direction: 2to1
qos:
history: keep_last
depth: 1
durability: transient_local
-
topic: /environment/base_map

- topic: /environment/base_map
type: autoware_lanelet2_msgs/msg/MapBin
queue_size: 1
direction: 2to1
qos:
history: keep_last
depth: 1
durability: transient_local
-
topic: /hardware_interface/controller/robot_status

- topic: /hardware_interface/controller/robot_status
type: carma_perception_msgs/msg/RobotEnabled
queue_size: 10
-
topic: /environment/external_objects

- topic: /environment/external_objects
type: carma_perception_msgs/msg/ExternalObjectList
queue_size: 1
-
topic: /environment/external_objects_viz

- topic: /environment/external_objects_viz
type: visualization_msgs/msg/MarkerArray
queue_size: 1
-
topic: /environment/roadway_obstacles_viz

- topic: /environment/roadway_obstacles_viz
type: visualization_msgs/msg/MarkerArray
queue_size: 1
-
topic: /environment/semantic_map

- topic: /environment/motion_computation_visualize
type: geometry_msgs/msg/PoseArray
queue_size: 1

- topic: /environment/semantic_map
type: autoware_lanelet2_msgs/msg/MapBin
queue_size: 1
direction: 2to1
qos:
history: keep_last
depth: 1
durability: transient_local
-
topic: /environment/map_update

- topic: /environment/map_update
type: autoware_lanelet2_msgs/msg/MapBin
queue_size: 200
direction: 2to1
qos:
history: keep_all
durability: transient_local
-
topic: /environment/external_object_predictions

- topic: /environment/external_object_predictions
type: carma_perception_msgs/msg/ExternalObjectList
queue_size: 1

-
topic: /hardware_interface/comms/outbound_binary_msg

- topic: /hardware_interface/comms/outbound_binary_msg
type: carma_driver_msgs/msg/ByteArray
queue_size: 100
direction: 2to1

-
topic: /hardware_interface/comms/inbound_binary_msg

- topic: /hardware_interface/comms/inbound_binary_msg
type: carma_driver_msgs/msg/ByteArray
queue_size: 100
direction: 1to2

-
topic: /system_alert
- topic: /system_alert
type: carma_msgs/msg/SystemAlert
queue_size: 5
qos:
history: keep_last
depth: 5
durability: transient_local
-
topic: /localization/vehicle/odom

- topic: /localization/vehicle/odom
type: nav_msgs/msg/Odometry
queue_size: 1
direction: 2to1
services_1_to_2: []

- topic: /environment/full_detection_list
type: carma_cooperative_perception_interfaces/msg/DetectionList
queue_size: 1

- topic: /environment/filtered_detection_list
type: carma_cooperative_perception_interfaces/msg/DetectionList
queue_size: 1

- topic: /environment/cooperative_perception_track_list
type: carma_cooperative_perception_interfaces/msg/TrackList
queue_size: 1

- topic: /environment/fused_external_objects
type: carma_perception_msgs/msg/ExternalObjectList
queue_size: 1

- topic: /environment/fused_external_objects_viz
type: visualization_msgs/msg/MarkerArray
queue_size: 1

- topic: /message/incoming_sdsm
type: carma_v2x_msgs/msg/SensorDataSharingMessage
queue_size: 1

- topic: /message/incoming_spat
type: carma_v2x_msgs/msg/SPAT
queue_size: 1

- topic: /message/incoming_map
type: carma_v2x_msgs/msg/MapData
queue_size: 1

- topic: /environment/output/markers
type: visualization_msgs/msg/MarkerArray
queue_size: 1

- topic: /guidance/ctrl_raw
type: autoware_msgs/msg/ControlCommandStamped
queue_size: 1

- topic: /guidance/ctrl_cmd
type: autoware_msgs/msg/ControlCommandStamped
queue_size: 1

- topic: /guidance/final_maneuver_plan
type: carma_planning_msgs/msg/ManeuverPlan
queue_size: 1

- topic: /guidance/plugins/pure_pursuit/plan_trajectory
type: carma_planning_msgs/msg/TrajectoryPlan
queue_size: 1

- topic: /guidance/route_state
type: carma_planning_msgs/msg/RouteState
queue_size: 1
direction: 2to1

services_1_to_2: []
2 changes: 1 addition & 1 deletion ail_vru_uc1_scenario/carma_rosconsole.conf
Original file line number Diff line number Diff line change
Expand Up @@ -23,4 +23,4 @@ log4j.logger.ros.inlanecruising_plugin=INFO
log4j.logger.ros.route_following_plugin=INFO
log4j.logger.ros.pure_pursuit_wrapper=INFO
log4j.logger.ros.system_controller=INFO
log4j.logger.ros.subsystem_controllers=INFO
log4j.logger.ros.subsystem_controllers=INFO
Original file line number Diff line number Diff line change
@@ -0,0 +1,152 @@
<!--
Copyright (C) 2021 LEIDOS.
Licensed under the Apache License, Version 2.0 (the "License"); you may not
@@ -13,6 +12,18 @@
WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
License for the specific language governing permissions and limitations under
the License.
This file is loosely based on the reference architecture developed by Intel Corporation for Leidos located here
https://github.com/carla-simulator/carla-autoware/blob/master/carla-autoware-agent/launch/carla_autoware_agent.launch
That file has the following license and some code snippets from it may be present in this file as well and are under the same license.
Copyright (c) 2019 Intel Corporation
This work is licensed under the terms of the MIT license.
For a copy, see <https://opensource.org/licenses/MIT>.
-->

<launch>
<!--
###################
## Configuration ##
###################
-->
<param name="use_sim_time" value="true"/>
<arg name='agent' default='agent'/>
<!-- connecting default info -->
<arg name='host' default='127.0.0.1'/>
<arg name='port' default='2000'/>
<arg name='town' default='Town04'/>

<!-- set synchronous_mode to 'true' when carma-carla-integration not running with co-simulation tool -->
<arg name='synchronous_mode' default='false' />
<arg name='synchronous_mode_wait_for_vehicle_control_command' default='false'/>
<arg name='fixed_delta_seconds' default='0.05'/>

<!-- use comma separated format "x,y,z,roll,pìtch,yaw" -->
<arg name='spawn_point' default='15.4,-90.1,0,0,0,90'/>

<!-- vehicle info -->
<arg name='role_name' default='carma_1'/>
<arg name='vehicle_model' default='vehicle.toyota.prius'/>
<arg name='vehicle_length' default='5.00'/>
<arg name='vehicle_width' default='3.00'/>
<arg name='vehicle_wheelbase' default='2.79'/>

<!-- vehicle speed PID -->
<arg name='speed_Kp' default='0.4'/>
<arg name='speed_Ki' default='0.03'/>
<arg name='speed_Kd' default='0.0'/>

<!-- vehicle acceleration PID -->
<arg name='accel_Kp' default='0.125'/>
<arg name='accel_Ki' default='0.0'/>
<arg name='accel_Kd' default='0.05'/>

<!-- Initial arguments for integration scripts -->
<arg name='init_speed' default='5'/>
<arg name='init_acceleration' default='1'/>
<arg name='init_steering_angle' default='0'/>
<arg name='init_jerk' default='0'/>
<arg name='max_steering_degree' default='70'/>

<arg name='use_ground_truth_localization' default='false'/>
<arg name='use_ground_truth_detections' default='false'/>

<!-- driver status params -->
<arg name='lidar_enabled' default='true'/>
<arg name='controller_enabled' default='true'/>
<arg name='camera_enabled' default='true'/>
<arg name='gnss_enabled' default='true'/>
<arg name='driver_status_pub_rate' default='10'/>

<!-- robot status params -->
<arg name='robot_status_pub_rate' default='10'/>

<!-- route and plugins params -->
<arg name='selected_route' default="Release_test_case_1"/>
<arg name="selected_plugins" default="['/guidance/plugins/route_following_plugin','/guidance/plugins/inlanecruising_plugin','/guidance/plugins/stop_and_wait_plugin','/guidance/plugins/pure_pursuit_wrapper']"/>
<arg name="start_delay_in_seconds" default="15.0"/>

<!-- enable sensor external object-->
<arg name='enable_sensor_objects' default='false'/>
<arg name='sensor_object_pub_rate' default='10'/>
<arg name='sensor_id' default="1"/>
<arg name='detection_cycle_delay_seconds' default='0.1'/>

<!--
##########################
## CARLA CARMA bridge ##
##########################
-->
<include file='$(find carma_carla_bridge)/launch/carma_carla_bridge.launch'>
<arg name='host' value='$(arg host)'/>
<arg name='port' value='$(arg port)'/>
<arg name='vehicle_filter' value='$(arg vehicle_model)'/>
<arg name='role_name' value='$(arg role_name)'/>
<arg name='spawn_point' value='$(arg spawn_point)'/>
<arg name='speed_Kp' value='$(arg speed_Kp)'/>
<arg name='speed_Ki' value='$(arg speed_Ki)'/>
<arg name='speed_Kd' value='$(arg speed_Kd)'/>
<arg name='accel_Kp' value='$(arg accel_Kp)'/>
<arg name='accel_Ki' value='$(arg accel_Ki)'/>
<arg name='accel_Kd' value='$(arg accel_Kd)'/>

<arg name='synchronous_mode' value='$(arg synchronous_mode)'/>
<arg name='synchronous_mode_wait_for_vehicle_control_command' value='$(arg synchronous_mode_wait_for_vehicle_control_command)'/>
<arg name='fixed_delta_seconds' value='$(arg fixed_delta_seconds)'/>

</include>

<!--
##################
## Agent bridge ##
##################
-->
<include file='$(find carma_carla_agent)/$(arg agent)/bridge.launch'>
<arg name='role_name' value='$(arg role_name)'/>
<arg name='wheelbase' value='$(arg vehicle_wheelbase)'/>
<arg name='init_speed' value='$(arg init_speed)'/>
<arg name='init_acceleration' value='$(arg init_acceleration)'/>
<arg name='init_steering_angle' value='$(arg init_steering_angle)'/>
<arg name='init_jerk' value='$(arg init_jerk)'/>
<arg name='max_steering_degree' value='$(arg max_steering_degree)'/>

<!-- driver status params -->
<arg name='lidar_enabled' value='$(arg lidar_enabled)'/>
<arg name='controller_enabled' value='$(arg controller_enabled)'/>
<arg name='camera_enabled' value='$(arg camera_enabled)'/>
<arg name='gnss_enabled' value='$(arg gnss_enabled)'/>
<arg name='driver_status_pub_rate' value='$(arg driver_status_pub_rate)'/>

<!-- robot status params -->
<arg name='robot_status_pub_rate' value='$(arg robot_status_pub_rate)'/>

<!-- route and plugins params-->
<arg name='selected_route' value='$(arg selected_route)'/>
<arg name='selected_plugins' value='$(arg selected_plugins)'/>
<arg name='start_delay_in_seconds' value='$(arg start_delay_in_seconds)'/>

<arg name='host' value="$(arg host)"/>
<arg name='port' value="$(arg port)"/>
<arg name='enable_sensor_objects' value='$(arg enable_sensor_objects)'/>
<arg name='sensor_object_pub_rate' value='$(arg sensor_object_pub_rate)'/>
<arg name='sensor_id' value='$(arg sensor_id)'/>
<arg name='detection_cycle_delay_seconds' value='$(arg detection_cycle_delay_seconds)'/>

</include>
</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,5 @@ curvature_moving_average_window_size: 9 # Size of the window used in the moving
back_distance: 20.0 # Number of meters behind the first maneuver that need to be included in points for curvature calculation
max_accel_multiplier: 0.4 # Multiplier of max_accel to bring the value under max_accel
lat_accel_multiplier: 0.15 # Multiplier of lat_accel to bring the value under lat_accel TODO needs to be optimized
enable_object_avoidance: true # Activates object avoidance logic
buffer_ending_downtrack: 20.0 # Additional distance beyond ending downtrack to ensure sufficient points
enable_object_avoidance: true # Activates object avoidance logic
buffer_ending_downtrack: 20.0 # Additional distance beyond ending downtrack to ensure sufficient points
Loading

0 comments on commit 109136d

Please sign in to comment.