From b520ec9fb9124caf92be6ed12a0b28f721f50c9e Mon Sep 17 00:00:00 2001 From: Alejandro Lopez Gestoso <alopez@iri.upc.edu> Date: Tue, 9 Mar 2021 17:10:06 +0100 Subject: [PATCH] Adapted to new config file distributions. Added bringup launch file --- calibration/basler_1280x960.yaml | 27 ++++ calibration/delock_1280x960.yaml | 20 +++ config/adc/basler_camera_config.yaml | 122 ++++++++++++++++++ config/adc/battery_config.yaml | 2 + config/adc/control_config.yaml | 15 +++ config/adc/delock_96368.yaml | 4 + config/adc/delock_camera_config.yaml | 5 + config/adc/egomotion_config.yaml | 4 + config/adc/odometry_config.yaml | 15 +++ config/adc/rplidar_config.yaml | 6 + config/adc/ultrasounds_config.yaml | 7 + config/adc_common/local_ekf_odom_imu.yaml | 39 ++++++ config/adc_common/mux.yaml | 20 +++ .../basler_camera_sim_config.yaml | 4 +- .../delock_camera_sim_config.yaml | 0 .../mpu9250_imu_config.yaml | 22 ++-- .../rear_center_sonar_config.yaml | 2 +- .../rear_left_sonar_config.yaml | 2 +- .../rear_right_sonar_config.yaml | 2 +- .../{adc_car => adc_sim}/rp_lidar_config.yaml | 4 +- .../side_left_sonar_config.yaml | 2 +- .../side_right_sonar_config.yaml | 2 +- launch/adc_bringup.launch | 23 ++++ 23 files changed, 329 insertions(+), 20 deletions(-) create mode 100644 calibration/basler_1280x960.yaml create mode 100644 calibration/delock_1280x960.yaml create mode 100644 config/adc/basler_camera_config.yaml create mode 100644 config/adc/battery_config.yaml create mode 100644 config/adc/control_config.yaml create mode 100644 config/adc/delock_96368.yaml create mode 100644 config/adc/delock_camera_config.yaml create mode 100644 config/adc/egomotion_config.yaml create mode 100644 config/adc/odometry_config.yaml create mode 100644 config/adc/rplidar_config.yaml create mode 100644 config/adc/ultrasounds_config.yaml create mode 100644 config/adc_common/local_ekf_odom_imu.yaml create mode 100644 config/adc_common/mux.yaml rename config/{adc_car => adc_sim}/basler_camera_sim_config.yaml (93%) rename config/{adc_car => adc_sim}/delock_camera_sim_config.yaml (100%) rename config/{adc_car => adc_sim}/mpu9250_imu_config.yaml (56%) rename config/{adc_car => adc_sim}/rear_center_sonar_config.yaml (79%) rename config/{adc_car => adc_sim}/rear_left_sonar_config.yaml (79%) rename config/{adc_car => adc_sim}/rear_right_sonar_config.yaml (79%) rename config/{adc_car => adc_sim}/rp_lidar_config.yaml (88%) rename config/{adc_car => adc_sim}/side_left_sonar_config.yaml (79%) rename config/{adc_car => adc_sim}/side_right_sonar_config.yaml (79%) create mode 100644 launch/adc_bringup.launch diff --git a/calibration/basler_1280x960.yaml b/calibration/basler_1280x960.yaml new file mode 100644 index 0000000..b9b7b76 --- /dev/null +++ b/calibration/basler_1280x960.yaml @@ -0,0 +1,27 @@ +image_width: 1280 +image_height: 960 +camera_name: basler +camera_matrix: + rows: 3 + cols: 3 + data: [ 614.21032, 0. , 609.0955 , + 0. , 586.95299, 474.1858 , + 0. , 0. , 1. ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.303012, 0.078540, -0.004561, -0.000561, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1.] +projection_matrix: + rows: 3 + cols: 4 + data: [ 452.98874, 0. , 608.18905, 0. , + 0. , 440.00012, 456.24679, 0. , + 0. , 0. , 1. , 0. ] + diff --git a/calibration/delock_1280x960.yaml b/calibration/delock_1280x960.yaml new file mode 100644 index 0000000..bcba3d1 --- /dev/null +++ b/calibration/delock_1280x960.yaml @@ -0,0 +1,20 @@ +image_width: 1280 +image_height: 960 +camera_name: camera +camera_matrix: + rows: 3 + cols: 3 + data: [744.492923, 0, 602.268315, 0, 743.285879, 502.094536, 0, 0, 1] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.324192, 0.08111499999999999, 0.002398, 0.000735, 0] +rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] +projection_matrix: + rows: 3 + cols: 4 + data: [530.792297, 0, 597.76504, 0, 0, 617.536865, 517.314024, 0, 0, 0, 1, 0] diff --git a/config/adc/basler_camera_config.yaml b/config/adc/basler_camera_config.yaml new file mode 100644 index 0000000..74297a8 --- /dev/null +++ b/config/adc/basler_camera_config.yaml @@ -0,0 +1,122 @@ +startup_user_set: "CurrentSetting" +# The tf frame under which the images were published +camera_frame: model_car/front_camera_uvc_camera_optical + +# The DeviceUserID of the camera. If empty, the first camera found in the +# device list will be used +device_user_id: "" +#device_user_id: "basler" + +# The CameraInfo URL (Uniform Resource Locator) where the optional intrinsic +# camera calibration parameters are stored. This URL string will be parsed +# from the ROS-CameraInfoManager: +# http://docs.ros.org/api/camera_info_manager/html/classcamera__info__manager_ +# 1_1CameraInfoManager.html#details +# camera_info_url: "" +camera_info_url: "file:///home/adc/model_car_ws/src/platforms/model_car/iri_model_car_bringup/calibration/basler_1280x960.yaml" + +# The encoding of the pixels -- channel meaning, ordering, size +# taken from the list of strings in include/sensor_msgs/image_encodings.h +# The supported encodings are 'mono8', 'bgr8', 'rgb8', 'bayer_bggr8', +# 'bayer_gbrg8' and 'bayer_rggb8' +# Default values are 'mono8' and 'rgb8' +image_encoding: "rgb8" + +# Binning factor to get downsampled images. It refers here to any camera +# setting which combines rectangular neighborhoods of pixels into larger +# "super-pixels." It reduces the resolution of the output image to +# (width / binning_x) x (height / binning_y). +# The default values binning_x = binning_y = 0 are considered the same +# as binning_x = binning_y = 1 (no subsampling). +# binning_x: 1 +# binning_y: 1 + +# The desired publisher frame rate if listening to the topics. +# This parameter can only be set once at startup +# Calling the GrabImages-Action can result in a higher framerate +frame_rate: 15.0 + +# Mode of camera's shutter. +# The supported modes are "rolling", "global" and "global_reset" +# Default value is "" (empty) means default_shutter_mode +shutter_mode: "global" + +########################################################################## +######################## Image Intensity Settings ######################## +########################################################################## +# The following settings do *NOT* have to be set. Each camera has default +# values which provide an automatic image adjustment resulting in valid +# images +########################################################################## + +# The exposure time in microseconds to be set after opening the camera. +# exposure: 10000.0 + +# The target gain in percent of the maximal value the camera supports +# For USB-Cameras, the gain is in dB, for GigE-Cameras it is given in so +# called 'device specific units'. +# gain: 0.5 + +# Gamma correction of pixel intensity. +# Adjusts the brightness of the pixel values output by the camera's sensor +# to account for a non-linearity in the human perception of brightness or +# of the display system (such as CRT). +# gamma: 1.0 + +# The average intensity value of the images. It depends the exposure time +# as well as the gain setting. If 'exposure' is provided, the interface will +# try to reach the desired brightness by only varying the gain. (What may +# often fail, because the range of possible exposure vaules is many +# times higher than the gain range). If 'gain' is provided, the interface will +# try to reach the desired brightness by only varying the exposure time. If +# gain AND exposure are given, it is not possible to reach the brightness, +# because both are assumed to be fix. +# brightness: 100 + +# Only relevant, if 'brightness' is set: +# The brightness_continuous flag controls the auto brightness function. +# If it is set to false, the brightness will only be reached once. +# Hence changing light conditions lead to changing brightness values. +# If it is set to true, the given brightness will be reached continuously, +# trying to adapt to changing light conditions. This is only possible for +# values in the possible auto range of the pylon API which is e.g. [50 - 205] +# for acA2500-14um and acA1920-40gm +# brightness_continuous: true + +# Only relevant, if 'brightness' is set: +# If the camera should try to reach and / or keep the brightness, hence +# adapting to changing light conditions, at least one of the following flags +# must be set. +# If both are set, the interface will use the profile that tries to keep the +# gain at minimum to reduce white noise. +# The exposure_auto flag indicates, that the desired brightness will be +# reached by adapting the exposure time. +# The gain_auto flag indicates, that the desired brightness will be +# reached by adapting the gain. +# exposure_auto: true +# gain_auto: true + +########################################################################## + +# The timeout while searching the exposure which is connected to the +# desired brightness. For slow system this has to be increased. +# exposure_search_timeout: 5.0 + +# The exposure search can be limited with an upper bound. This is to prevent +# very high exposure times and resulting timeouts. +# A typical value for this upper bound is ~2000000us. +# auto_exposure_upper_limit: 2000000.0 + +# The MTU size. Only used for GigE cameras. +# To prevent lost frames configure the camera has to be configured +# with the MTU size the network card supports. A value greater 3000 +# should be good (1500 for RaspberryPI) +# gige: +# mtu_size: 3000 + +# Only used for GigE cameras. +# The inter-package delay in ticks to prevent lost frames. +# For most of GigE-Cameras, a value of 1000 is reasonable. +# For cameras used on a RaspberryPI this value should be set to 11772. +# gige: +# inter_pkg_delay: 1000 diff --git a/config/adc/battery_config.yaml b/config/adc/battery_config.yaml new file mode 100644 index 0000000..710347b --- /dev/null +++ b/config/adc/battery_config.yaml @@ -0,0 +1,2 @@ +rate: 100 + diff --git a/config/adc/control_config.yaml b/config/adc/control_config.yaml new file mode 100644 index 0000000..6f4adee --- /dev/null +++ b/config/adc/control_config.yaml @@ -0,0 +1,15 @@ +rate: 40 +speed_Kp: 20.0 +speed_Ki: 50.0 +speed_Kd: 0.00 +speed_i_max: 20.0 +speed_deadband: 0.1 +watchdog_time: 0.5 +axel_distance: 0.3662 +max_steer_angle: 0.4 +min_steer_angle: -0.4 +max_speed_control: 20.0 +min_speed_control: -20.0 +max_steer_control: 62.0 +min_steer_control: -77.0 + diff --git a/config/adc/delock_96368.yaml b/config/adc/delock_96368.yaml new file mode 100644 index 0000000..3e8f52b --- /dev/null +++ b/config/adc/delock_96368.yaml @@ -0,0 +1,4 @@ +vendor: '0x090c' +product: '0xf37d' +serial: "" +index: 0 diff --git a/config/adc/delock_camera_config.yaml b/config/adc/delock_camera_config.yaml new file mode 100644 index 0000000..801f26f --- /dev/null +++ b/config/adc/delock_camera_config.yaml @@ -0,0 +1,5 @@ +width: 1280 +height: 960 +frame_id: 'model_car/rear_camera_uvc_camera_optical' +video_mode: 'mjpeg' +frame_rate: 30.0 diff --git a/config/adc/egomotion_config.yaml b/config/adc/egomotion_config.yaml new file mode 100644 index 0000000..85c0669 --- /dev/null +++ b/config/adc/egomotion_config.yaml @@ -0,0 +1,4 @@ +rate: 100 +imu_frame_id: model_car/imu_mpu9250 +gyro_cal_samples: 100 + diff --git a/config/adc/odometry_config.yaml b/config/adc/odometry_config.yaml new file mode 100644 index 0000000..be7ed56 --- /dev/null +++ b/config/adc/odometry_config.yaml @@ -0,0 +1,15 @@ +rate: 40 +odom_frame: "model_car/odom" +robot_frame: "model_car/base_footprint" +encoder_ticks: 30 +wheel_diameter: 0.100 +wheel_distance: 0.216 +axel_distance: 0.3662 +filter_coeff: 0.3 +speed_deadband: 0.1 +max_steer_angle: 0.4 +min_steer_angle: -0.4 +max_steer_control: 62.0 +min_steer_control: -77.0 +publish_tf: False + diff --git a/config/adc/rplidar_config.yaml b/config/adc/rplidar_config.yaml new file mode 100644 index 0000000..4786aec --- /dev/null +++ b/config/adc/rplidar_config.yaml @@ -0,0 +1,6 @@ +serial_port: "/dev/ttyUSB0" +serial_baudrate: 115200 +frame_id: "model_car/front_lidar_rplidar_scan_frame" +inverted: false +angle_compensate: true + diff --git a/config/adc/ultrasounds_config.yaml b/config/adc/ultrasounds_config.yaml new file mode 100644 index 0000000..841ed22 --- /dev/null +++ b/config/adc/ultrasounds_config.yaml @@ -0,0 +1,7 @@ +rate: 100 +side_right_frame_id: model_car/side_right_ranger1d +rear_right_frame_id: model_car/rear_right_ranger1d +rear_center_frame_id: model_car/rear_center_ranger1d +rear_left_frame_id: model_car/rear_left_ranger1d +side_left_frame_id: model_car/side_left_ranger1d + diff --git a/config/adc_common/local_ekf_odom_imu.yaml b/config/adc_common/local_ekf_odom_imu.yaml new file mode 100644 index 0000000..30c2132 --- /dev/null +++ b/config/adc_common/local_ekf_odom_imu.yaml @@ -0,0 +1,39 @@ +frequency: 30 +sensor_timeout: 1.0 +transform_time_offset: 0.0 +two_d_mode: true +print_diagnostics: true +debug: false +debug_out_file: /tmp/debug_ekf_localization.txt +use_control: false +control_config: [true, false, false, false, false, true] + +publish_tf: true +map_frame: map +odom_frame: model_car/odom +base_link_frame: model_car/base_footprint +world_frame: model_car/odom + +odom0: /model_car/odom +odom0_config: [false, false, false, + false, false, false, + true, false, false, + false, false, true, + false, false, false] + +imu0: /model_car/sensors/imu_data +imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, true, + false, false, false] + +imu0_differential: false +imu0_relative: true +imu0_remove_gravitational_acceleration: true + +#meaning_config: [x, y, z, +# roll, pitch, yaw, +# vx, vy, vz, +# vroll, vpitch, vyaw, +# ax, ay, az] diff --git a/config/adc_common/mux.yaml b/config/adc_common/mux.yaml new file mode 100644 index 0000000..4275edf --- /dev/null +++ b/config/adc_common/mux.yaml @@ -0,0 +1,20 @@ +subscribers: + - name: "Default input" + topic: "/model_car/default/cmd_vel" + timeout: 0.1 + priority: 0 + short_desc: "The default cmd_vel, controllers unaware that we are multiplexing cmd_vel should come here" + + - name: "Navigation stack" + topic: "/model_car/navigation/cmd_vel" + timeout: 0.5 + priority: 5 + short_desc: "Navigation stack controller" + + - name: "Teleop" + topic: "/model_car/teleop/cmd_vel" + timeout: 0.1 + priority: 10 + short_desc: "Teleoperation" + +publisher: "/model_car/cmd_vel" diff --git a/config/adc_car/basler_camera_sim_config.yaml b/config/adc_sim/basler_camera_sim_config.yaml similarity index 93% rename from config/adc_car/basler_camera_sim_config.yaml rename to config/adc_sim/basler_camera_sim_config.yaml index 2fbe6d5..56ffb8c 100644 --- a/config/adc_car/basler_camera_sim_config.yaml +++ b/config/adc_sim/basler_camera_sim_config.yaml @@ -1,6 +1,6 @@ horizontal_fov: 2.25 -width: 1280 -height: 960 +width: 640 +height: 480 format: 'R8G8B8' clip_near: 0.1 clip_far: 100 diff --git a/config/adc_car/delock_camera_sim_config.yaml b/config/adc_sim/delock_camera_sim_config.yaml similarity index 100% rename from config/adc_car/delock_camera_sim_config.yaml rename to config/adc_sim/delock_camera_sim_config.yaml diff --git a/config/adc_car/mpu9250_imu_config.yaml b/config/adc_sim/mpu9250_imu_config.yaml similarity index 56% rename from config/adc_car/mpu9250_imu_config.yaml rename to config/adc_sim/mpu9250_imu_config.yaml index d95002f..52d7790 100644 --- a/config/adc_car/mpu9250_imu_config.yaml +++ b/config/adc_sim/mpu9250_imu_config.yaml @@ -1,6 +1,6 @@ rate: 100.0 -namespace: 'adc_car' -tf_prefix: 'adc_car' +namespace: 'model_car' +tf_prefix: 'model_car' imu_topic: 'sensors/imu' accel_offset_x: 0.06 accel_offset_y: 0.06 @@ -11,15 +11,15 @@ accel_drift_z: 0.001 accel_noise_x: 0.008 accel_noise_y: 0.008 accel_noise_z: 0.008 -rate_offset_x: 5.0 -rate_offset_y: 5.0 -rate_offset_z: 5.0 -rate_drift_x: 0.015 -rate_drift_y: 0.015 -rate_drift_z: 0.015 -rate_noise_x: 0.1 -rate_noise_y: 0.1 -rate_noise_z: 0.1 +rate_offset_x: 0.0 +rate_offset_y: 0.0 +rate_offset_z: 0.0 +rate_drift_x: 0.0015 +rate_drift_y: 0.0015 +rate_drift_z: 0.0015 +rate_noise_x: 0.01 +rate_noise_y: 0.01 +rate_noise_z: 0.01 heading_offset: 0.0 heading_drift: 0.0 heading_noise: 0.0 diff --git a/config/adc_car/rear_center_sonar_config.yaml b/config/adc_sim/rear_center_sonar_config.yaml similarity index 79% rename from config/adc_car/rear_center_sonar_config.yaml rename to config/adc_sim/rear_center_sonar_config.yaml index c63338f..c57811c 100644 --- a/config/adc_car/rear_center_sonar_config.yaml +++ b/config/adc_sim/rear_center_sonar_config.yaml @@ -4,7 +4,7 @@ fov: 0.5 min_range: 0.2 max_range: 4.0 resolution: 0.003 -frame_id: 'adc_car/rear_center_ranger1d' +frame_id: 'model_car/rear_center_ranger1d' ranger1d_topic: 'sensors/range/rear_center_sonar' type: 'ultrasound' noise: 0.01 diff --git a/config/adc_car/rear_left_sonar_config.yaml b/config/adc_sim/rear_left_sonar_config.yaml similarity index 79% rename from config/adc_car/rear_left_sonar_config.yaml rename to config/adc_sim/rear_left_sonar_config.yaml index 4a17277..eb9e61e 100644 --- a/config/adc_car/rear_left_sonar_config.yaml +++ b/config/adc_sim/rear_left_sonar_config.yaml @@ -4,7 +4,7 @@ fov: 0.5 min_range: 0.2 max_range: 4.0 resolution: 0.003 -frame_id: 'adc_car/rear_left_ranger1d' +frame_id: 'model_car/rear_left_ranger1d' ranger1d_topic: 'sensors/range/rear_left_sonar' type: 'ultrasound' noise: 0.01 diff --git a/config/adc_car/rear_right_sonar_config.yaml b/config/adc_sim/rear_right_sonar_config.yaml similarity index 79% rename from config/adc_car/rear_right_sonar_config.yaml rename to config/adc_sim/rear_right_sonar_config.yaml index 96a2039..ed02ce3 100644 --- a/config/adc_car/rear_right_sonar_config.yaml +++ b/config/adc_sim/rear_right_sonar_config.yaml @@ -4,7 +4,7 @@ fov: 0.5 min_range: 0.2 max_range: 4.0 resolution: 0.003 -frame_id: 'adc_car/rear_right_ranger1d' +frame_id: 'model_car/rear_right_ranger1d' ranger1d_topic: 'sensors/range/rear_right_sonar' type: 'ultrasound' noise: 0.01 diff --git a/config/adc_car/rp_lidar_config.yaml b/config/adc_sim/rp_lidar_config.yaml similarity index 88% rename from config/adc_car/rp_lidar_config.yaml rename to config/adc_sim/rp_lidar_config.yaml index 02c79f3..38a70a4 100644 --- a/config/adc_car/rp_lidar_config.yaml +++ b/config/adc_sim/rp_lidar_config.yaml @@ -8,8 +8,8 @@ visualize: false update_rate: 10 samples: 400 resolution: 1 -min_angle: -1.5708 -max_angle: 1.5708 +min_angle: 1.5708 +max_angle: 4.71229 range_min: 0.15 range_max: 6.0 range_resolution: 0.05 diff --git a/config/adc_car/side_left_sonar_config.yaml b/config/adc_sim/side_left_sonar_config.yaml similarity index 79% rename from config/adc_car/side_left_sonar_config.yaml rename to config/adc_sim/side_left_sonar_config.yaml index fbcf3c6..0cb6214 100644 --- a/config/adc_car/side_left_sonar_config.yaml +++ b/config/adc_sim/side_left_sonar_config.yaml @@ -4,7 +4,7 @@ fov: 0.5 min_range: 0.2 max_range: 4.0 resolution: 0.003 -frame_id: 'adc_car/side_left_ranger1d' +frame_id: 'model_car/side_left_ranger1d' ranger1d_topic: 'sensors/range/side_left_sonar' type: 'ultrasound' noise: 0.01 diff --git a/config/adc_car/side_right_sonar_config.yaml b/config/adc_sim/side_right_sonar_config.yaml similarity index 79% rename from config/adc_car/side_right_sonar_config.yaml rename to config/adc_sim/side_right_sonar_config.yaml index c1bb4a1..050078c 100644 --- a/config/adc_car/side_right_sonar_config.yaml +++ b/config/adc_sim/side_right_sonar_config.yaml @@ -4,7 +4,7 @@ fov: 0.5 min_range: 0.2 max_range: 4.0 resolution: 0.003 -frame_id: 'adc_car/side_right_ranger1d' +frame_id: 'model_car/side_right_ranger1d' ranger1d_topic: 'sensors/range/side_right_sonar' type: 'ultrasound' noise: 0.01 diff --git a/launch/adc_bringup.launch b/launch/adc_bringup.launch new file mode 100644 index 0000000..bcc4cb1 --- /dev/null +++ b/launch/adc_bringup.launch @@ -0,0 +1,23 @@ +<?xml version="1.0" encoding="UTF-8"?> +<launch> + <arg name="name" default="adc_car"/> + <arg name="output" default="screen"/> + <arg name="launch_prefix" default=""/> + + <include file="$(find iri_model_car_bringup)/launch/bringup.launch"> + <arg name="car_name" value="$(arg name)"/> + <arg name="battery_config_file" value="$(find iri_adc_launch)/config/adc/battery_config.yaml"/> + <arg name="ultrasounds_config_file" value="$(find iri_adc_launch)/config/adc/ultrasounds_config.yaml"/> + <arg name="egomotion_config_file" value="$(find iri_adc_launch)/config/adc/egomotion_config.yaml"/> + <arg name="basler_config_file" value="$(find iri_adc_launch)/config/adc/basler_camera_config.yaml"/> + <arg name="delock_device_file" value="$(find iri_adc_launch)/config/adc/delock_96368.yaml" /> + <arg name="delock_format_file" value="$(find iri_adc_launch)/config/adc/delock_camera_config.yaml" /> + <arg name="delock_calib_file" value="$(find iri_adc_launch)/calibration/delock_1280x960.yaml" /> + <arg name="rplidar_config_file" value="$(find iri_adc_launch)/config/adc/rplidar_config.yaml" /> + <arg name="control_config_file" value="$(find iri_adc_launch)/config/adc/control_config.yaml"/> + <arg name="odometry_config_file" value="$(find iri_adc_launch)/config/adc/odometry_config.yaml"/> + <arg name="cmd_vel_mux_config_file" value="$(find iri_adc_launch)/config/adc_common/mux.yaml"/> + <arg name="local_ekf_config_file" value="$(find iri_adc_launch)/config/adc_common/local_ekf_odom_imu.yaml"/> + <arg name="sim_config_path" value="$(find iri_adc_launch)/config/adc_sim"/> + </include> +</launch> -- GitLab