-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsensors.launch
93 lines (83 loc) · 5.51 KB
/
sensors.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
<launch>
<arg name="nodelet_manager_on" default="true" />
<arg name="nodelet_manager_name" default="sensors_nodelet_manager" />
<arg name="baro_on" default="true" />
<arg name="baro_path" default="/dev/spidev2.0" />
<arg name="baro_frame_id" default="bmp390" />
<arg name="baro_poll_rate" default="60.0" />
<arg name="imu_on" default="true" />
<arg name="imu_path_acc" default="/dev/spidev0.0" />
<arg name="imu_path_gyro" default="/dev/spidev0.1" />
<arg name="imu_frame_id" default="bmi088" />
<arg name="imu_poll_rate" default="500.0" />
<arg name="imu_bias_samples" default="2000" />
<arg name="radar_on" default="true" />
<arg name="radar_path_cfg" default="/dev/ttyUSB0" />
<arg name="radar_path_data" default="/dev/ttyUSB1" />
<arg name="radar_frame_id" default="awr1843aop" />
<arg name="radar_poll_rate" default="8.0" />
<arg name="radar_cfg" default="$(find mav_sensors_demo)/cfg/radar/xwr18xx_AOP_profile_best_velocity_resolution.cfg" />
<arg name="radar_trigger" default="true" />
<arg name="radar_trigger_delay" default="500" />
<arg name="radar_trigger_gpio" default="389" />
<arg name="radar_trigger_gpio_name" default="PG.06" />
<arg name="imu_filter_on" default="true" />
<arg name="cam_on" default="false" />
<arg name="cam_frame_id" default="cam" />
<arg name="cam_name" default="cam" />
<arg name="cam_sensor_id" default="0" />
<arg name="cam_width" default="1640" />
<arg name="cam_height" default="1232" />
<arg name="cam_fps" default="30" />
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager" cwd="node" output="screen" if="$(arg nodelet_manager_on)"/>
<node pkg="nodelet" type="nodelet" name="baro" args="load mav_sensors_ros::BaroNodelet $(arg nodelet_manager_name)" output="screen" if="$(arg baro_on)">
<param name="path" value="$(arg baro_path)"/>
<param name="frame_id" value="$(arg baro_frame_id)"/>
<param name="poll_rate" value="$(arg baro_poll_rate)"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu" args="load mav_sensors_ros::ImuNodelet $(arg nodelet_manager_name)" output="screen" if="$(arg imu_on)">
<rosparam file="$(find mav_sensors_ros)/cfg/imu.yaml" />
<param name="path_acc" value="$(arg imu_path_acc)"/>
<param name="path_gyro" value="$(arg imu_path_gyro)"/>
<param name="frame_id" value="$(arg imu_frame_id)"/>
<param name="poll_rate" value="$(arg imu_poll_rate)"/>
<param name="bias_samples" value="$(arg imu_bias_samples)"/>
</node>
<group if="$(arg radar_on)">
<node pkg="nodelet" type="nodelet" name="radar" args="load mav_sensors_ros::RadarNodelet $(arg nodelet_manager_name)" output="screen">
<param name="path_cfg" value="$(arg radar_path_cfg)"/>
<param name="path_data" value="$(arg radar_path_data)"/>
<param name="frame_id" value="$(arg radar_frame_id)"/>
<param name="poll_rate" value="$(arg radar_poll_rate)"/>
<param name="radar_cfg" value="$(arg radar_cfg)"/>
<param name="trigger" type="string" value="$(arg radar_trigger)"/>
<param name="trigger_delay" type="string" value="$(arg radar_trigger_delay)"/>
<param name="trigger_gpio" type="string" value="$(arg radar_trigger_gpio)"/>
<param name="trigger_gpio_name" type="string" value="$(arg radar_trigger_gpio_name)"/>
</node>
</group>
<group if="$(arg imu_filter_on)">
<node pkg="nodelet" type="nodelet" name="ImuFilterNodelet" args="load imu_filter_madgwick/ImuFilterNodelet $(arg nodelet_manager_name)" output="screen">
<param name="publish_tf" value="false"/>
<param name="use_mag" value="false"/>
</node>
</group>
<group if="$(arg cam_on)">
<node pkg="nodelet" type="nodelet" name="cam_nodelet_manager" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="GSCamNodelet" args="load gscam/GSCamNodelet cam_nodelet_manager" output="screen">
<param name="camera_name" value="$(arg cam_name)" />
<param name="camera_info_url" value="file://$(find mav_sensors_ros)/cfg/cam.yaml"/>
<param name="gscam_config" value="nvarguscamerasrc sensor-id=$(arg cam_sensor_id) ! video/x-raw(memory:NVMM), width=(int)$(arg cam_width), height=(int)$(arg cam_height), format=(string)NV12, framerate=(fraction)$(arg cam_fps)/1 ! nvvidconv flip-method=0 ! video/x-raw, width=(int)820, height=(int)616, format=(string)BGRx ! videoconvert"/>
<param name="frame_id" value="$(arg cam_frame_id)" />
<param name="sync_sink" value="true" />
<remap from="camera/image_raw" to="$(arg cam_name)/image_raw" />
<remap from="/set_camera_info" to="$(arg cam_name)/set_camera_info" />
</node>
<node pkg="nodelet" type="nodelet" name="rectify" args="load image_proc/rectify cam_nodelet_manager" output="screen">
<remap from="image_mono" to="$(arg cam_name)/image_raw"/>
<remap from="image_rect" to="$(arg cam_name)/image_rect"/>
</node>
</group>
<node name="radar_to_cam_broadcaster" pkg="tf2_ros" type="static_transform_publisher" args="-0.020 -0.015 0.0 0.0 0.7071068 0.7071068 0.0 '$(arg radar_frame_id)' '$(arg cam_frame_id)'" />
<node name="imu_to_radar_broadcaster" pkg="tf2_ros" type="static_transform_publisher" args="0.122 0.000 -0.025 0.67620958 0.67620958 -0.20673802 -0.20673802 '$(arg imu_frame_id)' '$(arg radar_frame_id)'" />
</launch>