Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: throttled the radar inputs #371

Open
wants to merge 2 commits into
base: tier4/universe
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions aip_x2_gen2_launch/config/ARS548.param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,8 @@
sensor_model: ARS548
configuration_host_port: 42401
configuration_sensor_port: 42101
use_sensor_time: false
configuration_vehicle_length: 7.2369
configuration_vehicle_width: 2.2916
use_sensor_time: true
configuration_vehicle_length: 7.2369 # wheel_base + front_overhang + rear_overhang
configuration_vehicle_width: 2.2916 # wheel_tread + left_overhang + right_overhang
configuration_vehicle_height: 3.08
configuration_vehicle_wheelbase: 4.76012
44 changes: 26 additions & 18 deletions aip_x2_gen2_launch/launch/radar.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -7,16 +7,24 @@
<let name="steering_angle_topic" value="/vehicle/status/steering_status_scalar"/>
<arg name="radar_tracks_msgs_converter_param_path" default="$(find-pkg-share common_sensor_launch)/config/radar_tracks_msgs_converter.param.yaml"/>

<let name="odometry_throttled_topic" value="$(var odometry_topic)_throttled"/>
<let name="acceleration_throttled_topic" value="$(var acceleration_topic)_throttled"/>
<let name="steering_angle_throttled_topic" value="$(var steering_angle_topic)_throttled"/>

<node pkg="topic_tools" exec="throttle" name="odometry_throttler" output="screen" args="messages $(var odometry_topic) 25.0 $(var odometry_throttled_topic)"/>
<node pkg="topic_tools" exec="throttle" name="acceleration_throttler" output="screen" args="messages $(var acceleration_topic) 25.0 $(var acceleration_throttled_topic)"/>
<node pkg="topic_tools" exec="throttle" name="steering_angle_throttler" output="screen" args="messages $(var steering_angle_topic) 25.0 $(var steering_angle_throttled_topic)"/>

<group>
<push-ros-namespace namespace="radar"/>
<node pkg="topic_tools" exec="transform" name="steer_angle_transform" output="screen" args="/vehicle/status/steering_status /vehicle/status/steering_status_scalar std_msgs/msg/Float32 'std_msgs.msg.Float32(data=m.steering_tire_angle)' --import autoware_vehicle_msgs std_msgs --wait-for-start"/>

<group>
<push-ros-namespace namespace="front_center"/>
<include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
<arg name="odometry_topic" value="$(var odometry_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
<arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
<arg name="launch_hw" value="$(var launch_driver)"/>

<arg name="sensor_ip" value="10.13.1.114"/>
Expand Down Expand Up @@ -45,9 +53,9 @@
<group>
<push-ros-namespace namespace="front_left"/>
<include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
<arg name="odometry_topic" value="$(var odometry_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
<arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
<arg name="launch_hw" value="$(var launch_driver)"/>

<arg name="sensor_ip" value="10.13.1.115"/>
Expand Down Expand Up @@ -75,9 +83,9 @@
<group>
<push-ros-namespace namespace="front_right"/>
<include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
<arg name="odometry_topic" value="$(var odometry_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
<arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
<arg name="launch_hw" value="$(var launch_driver)"/>

<arg name="sensor_ip" value="10.13.1.116"/>
Expand Down Expand Up @@ -105,9 +113,9 @@
<group>
<push-ros-namespace namespace="rear_center"/>
<include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
<arg name="odometry_topic" value="$(var odometry_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
<arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
<arg name="launch_hw" value="$(var launch_driver)"/>

<arg name="sensor_ip" value="10.13.1.117"/>
Expand Down Expand Up @@ -135,9 +143,9 @@
<group>
<push-ros-namespace namespace="rear_left"/>
<include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
<arg name="odometry_topic" value="$(var odometry_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
<arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
<arg name="launch_hw" value="$(var launch_driver)"/>

<arg name="sensor_ip" value="10.13.1.118"/>
Expand Down Expand Up @@ -165,9 +173,9 @@
<group>
<push-ros-namespace namespace="rear_right"/>
<include file="$(find-pkg-share aip_x2_gen2_launch)/launch/ars548.launch.xml">
<arg name="odometry_topic" value="$(var odometry_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_topic)"/>
<arg name="odometry_topic" value="$(var odometry_throttled_topic)"/>
<arg name="acceleration_topic" value="$(var acceleration_throttled_topic)"/>
<arg name="steering_angle_topic" value="$(var steering_angle_throttled_topic)"/>
<arg name="launch_hw" value="$(var launch_driver)"/>

<arg name="sensor_ip" value="10.13.1.119"/>
Expand Down
Loading