Skip to content

Commit 37219e2

Browse files
jhughes50jhughes50
committed
add launch file from drone
1 parent f15d7be commit 37219e2

File tree

1 file changed

+286
-0
lines changed

1 file changed

+286
-0
lines changed

‎glider/launch/iapetus-sm.launch.py‎

Lines changed: 286 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,286 @@
1+
"""
2+
Jason Hughes
3+
January 2025
4+
5+
Launch the factor graph node
6+
"""
7+
8+
import os
9+
import launch
10+
11+
import ament_index_python.packages
12+
13+
from launch.actions import DeclareLaunchArgument as LaunchArg
14+
from launch.actions import OpaqueFunction
15+
from launch.substitutions import LaunchConfiguration as LaunchConfig
16+
from launch.substitutions import PathJoinSubstitution as PJoin
17+
from launch_ros.actions import ComposableNodeContainer, Node
18+
from launch_ros.descriptions import ComposableNode
19+
from launch_ros.substitutions import FindPackageShare
20+
from ament_index_python.packages import get_package_share_directory
21+
22+
23+
import yaml
24+
25+
26+
# --------- master_example.launch.py
27+
28+
29+
camera_list = {
30+
'cam0': '23540201',
31+
}
32+
33+
serial = '23540201'
34+
camera_type = 'blackfly_s'
35+
parameter_file = PJoin(
36+
[FindPackageShare('spinnaker_camera_driver'), 'config', camera_type + '.yaml']
37+
)
38+
39+
exposure_controller_parameters = {
40+
'type': 'master',
41+
'brightness_target': 120, # from 0..255
42+
'brightness_tolerance': 20, # when to update exposure/gain
43+
# watch that max_exposure_time is short enough
44+
# to support the trigger frame rate!
45+
'max_exposure_time': 15000, # usec
46+
'min_exposure_time': 5000, # usec
47+
'max_gain': 29.9,
48+
'gain_priority': False,
49+
}
50+
51+
camera_params = {
52+
'debug': False,
53+
'quiet': True,
54+
'buffer_queue_size': 1,
55+
'compute_brightness': True,
56+
'exposure_auto': 'Continuous',
57+
'exposure_time': 10000, # not used under auto exposure
58+
'trigger_mode': 'Off',
59+
'frame_rate_auto': 'Off',
60+
'frame_rate_enable': True,
61+
'gain_auto': 'Continuous',
62+
'trigger_source': 'Software',
63+
'trigger_selector': 'FrameStart',
64+
'trigger_overlap': 'ReadOut',
65+
'trigger_activation': 'RisingEdge',
66+
'balance_white_auto': 'Continuous',
67+
# You must enable chunk mode and chunks: frame_id, exposure_time, and gain
68+
'chunk_mode_active': True,
69+
'chunk_selector_frame_id': 'FrameID',
70+
'chunk_enable_frame_id': True,
71+
'chunk_selector_exposure_time': 'ExposureTime',
72+
'chunk_enable_exposure_time': True,
73+
'chunk_selector_gain': 'Gain',
74+
'chunk_enable_gain': True,
75+
# The Timestamp is not used at the moment
76+
'chunk_selector_timestamp': 'Timestamp',
77+
'chunk_enable_timestamp': True,
78+
'frame_rate': 5
79+
}
80+
81+
82+
def make_parameters(context):
83+
"""Launch synchronized camera driver node."""
84+
pd = LaunchConfig('camera_parameter_directory')
85+
calib_url = 'file://' + LaunchConfig('calibration_directory').perform(context) + '/'
86+
87+
exp_ctrl_names = [cam + '.exposure_controller' for cam in camera_list.keys()]
88+
driver_parameters = {
89+
'cameras': list(camera_list.keys()),
90+
'exposure_controllers': exp_ctrl_names,
91+
'ffmpeg_image_transport.encoding': 'hevc_nvenc', # only for ffmpeg image transport
92+
}
93+
# generate identical exposure controller parameters for all cameras
94+
for exp in exp_ctrl_names:
95+
driver_parameters.update(
96+
{exp + '.' + k: v for k, v in exposure_controller_parameters.items()}
97+
)
98+
99+
# generate camera parameters
100+
cam_parameters['parameter_file'] = PJoin([pd, 'blackfly_s.yaml'])
101+
for cam, serial in camera_list.items():
102+
cam_params = {cam + '.' + k: v for k, v in cam_parameters.items()}
103+
cam_params[cam + '.serial_number'] = serial
104+
cam_params[cam + '.camerainfo_url'] = calib_url + serial + '.yaml'
105+
cam_params[cam + '.frame_id'] = cam
106+
driver_parameters.update(cam_params) # insert into main parameter list
107+
# link the camera to its exposure controller. Each camera has its own controller
108+
driver_parameters.update({cam + '.exposure_controller_name': cam + '.exposure_controller'})
109+
return driver_parameters
110+
111+
112+
# ----------------------------------
113+
114+
def launch_setup(context, *args, **kwargs):
115+
"""Create composable node."""
116+
# For the camera
117+
#cam_name = LaunchConfig("camera_name")
118+
#cam_str = cam_name.perform(context)
119+
120+
# For the GPS
121+
config_directory = os.path.join(
122+
ament_index_python.packages.get_package_share_directory('ublox_gps'),
123+
'config')
124+
param_config = os.path.join(config_directory, 'zed_f9p.yaml')
125+
with open(param_config, 'r') as f:
126+
params = yaml.safe_load(f)['ublox_gps_node']['ros__parameters']
127+
128+
# For EC
129+
# bias_config = os.path.join(
130+
# ament_index_python.packages.get_package_share_directory('high_altitude_ec'),
131+
# 'config/silky_ev_all_zero.bias')
132+
bias_config = "/home/dcist/fclad/ROS/high_altitude_env/src/high_altitude_ec/config/silkyHD_all_zero.bias"
133+
# Declare launch arguments
134+
135+
# Find package share directory
136+
glider_share = FindPackageShare('glider')
137+
glider_share_dir = get_package_share_directory('glider')
138+
139+
# Path to parameter files
140+
ros_params_file = PJoin([
141+
glider_share,
142+
'config',
143+
'ros-params.yaml'
144+
])
145+
146+
graph_params_file = PJoin([
147+
glider_share,
148+
'config',
149+
'vectornav-vn100t.yaml'
150+
])
151+
152+
container = ComposableNodeContainer(
153+
name="high_altitude_ec_container",
154+
namespace="",
155+
package="rclcpp_components",
156+
executable="component_container",
157+
# prefix=['xterm -e gdb -ex run --args'],
158+
composable_node_descriptions=[
159+
# Rosbag2 recorder
160+
ComposableNode(
161+
package='rosbag2_composable_recorder',
162+
plugin='rosbag2_composable_recorder::ComposableRecorder',
163+
name="recorder",
164+
parameters=[{'topics': [
165+
"/ublox_gps_node/fix",
166+
"/odom",
167+
"/vectornav/imu",
168+
"/vectornav/magnetic",
169+
"/cam_driver/image_raw"
170+
],
171+
'storage_id': 'mcap',
172+
'record_all': False,
173+
'disable_discovery': False,
174+
'serialization_format': 'cdr',
175+
'start_recording_immediately': False,
176+
"bag_name": LaunchConfig("bag"),
177+
"bag_prefix": LaunchConfig("bag_prefix")}],
178+
remappings=[],
179+
extra_arguments=[{'use_intra_process_comms': True}],
180+
),
181+
182+
# Vectornav Raw
183+
ComposableNode(
184+
package='vectornav',
185+
plugin='vectornav::Vectornav',
186+
name='vectornav',
187+
parameters=[PJoin(
188+
[FindPackageShare('vectornav'),
189+
'config', 'vectornav_composable.yaml'])],
190+
remappings=[],
191+
extra_arguments=[{'use_intra_process_comms': True}]),
192+
193+
# FLIR camera
194+
ComposableNode(
195+
package='spinnaker_camera_driver',
196+
plugin='spinnaker_camera_driver::CameraDriver',
197+
name="cam_driver",
198+
parameters=[camera_params, {'parameter_file': parameter_file, 'serial_number': serial}],
199+
remappings=[
200+
('~/control', '/exposure_control/control'),
201+
],
202+
extra_arguments=[{'use_intra_process_comms': True}],
203+
),
204+
# Ublox GPS
205+
ComposableNode(
206+
package='ublox_gps',
207+
plugin='ublox_node::UbloxNode',
208+
name='ublox_gps_node',
209+
parameters=[params],
210+
remappings=[("/aidalm", "/ublox_raw/aidalm"),
211+
("/timtm2", "/ublox_raw/timtm2"),
212+
("/rtcm", "/ublox_raw/rtcm"),
213+
("/nmea", "/ublox_raw/nmea"),
214+
("/navclock", "/ublox_raw/navclock"),
215+
("/navcov", "/ublox_raw/navcov"),
216+
("/navheading", "/ublox_raw/navheading"),
217+
("/navrelposned", "/ublox_raw/navrelposned"),
218+
("/navstate", "/ublox_raw/navstate"),
219+
("/navsvin", "/ublox_raw/navsvin"),
220+
("/navstatus", "/ublox_raw/navstatus"),
221+
("/aideph", "/ublox_raw/aideph"),
222+
("/diagnostics", "/ublox_raw/diagnostics"),
223+
("/monhw", "/ublox_raw/monhw"),
224+
("/navsin", "/ublox_raw/nmea"),
225+
("/rtcm", "/ublox_raw/rtcm"),
226+
("/rxmrtcm", "/ublox_raw/rxmrtcm")],
227+
extra_arguments=[{'use_intra_process_comms': True}],
228+
),
229+
ComposableNode(
230+
package='glider',
231+
plugin='GliderROS::GliderNode',
232+
name='glider_node',
233+
parameters=[
234+
ros_params_file,
235+
{'path': graph_params_file,
236+
'use_sim_time': False,
237+
'use_odom': False}
238+
],
239+
remappings=[("/gps", "/ublox_gps_node/fix"), ("/imu", "/vectornav/imu")],
240+
),
241+
242+
ComposableNode(
243+
package='vectornav',
244+
plugin='vectornav::VnSensorMsgs',
245+
name='vn_sensor_msgs',
246+
parameters=[PJoin(
247+
[FindPackageShare('vectornav'),
248+
'config', 'vn_sensor_msgs_composable.yaml'])],
249+
remappings=[],
250+
extra_arguments=[{'use_intra_process_comms': True}]
251+
),
252+
],
253+
output="screen",
254+
)
255+
return [container]
256+
257+
258+
259+
260+
def generate_launch_description():
261+
"""Create composable node by calling opaque function."""
262+
return launch.LaunchDescription(
263+
[
264+
LaunchArg("bag", default_value=[""], description="name of output bag"),
265+
LaunchArg("bag_prefix", default_value=["/home/dcist/data/symbiote_"], description="prefix of output bag"),
266+
# FLIR camera
267+
LaunchArg(
268+
'camera_parameter_directory',
269+
default_value=PJoin([FindPackageShare('spinnaker_camera_driver'), 'config']),
270+
description='root directory for camera parameter definitions',
271+
),
272+
LaunchArg(
273+
'calibration_directory',
274+
default_value=['camera_calibrations'],
275+
description='root directory for camera calibration files',
276+
),
277+
# This is for the composed nodes
278+
OpaqueFunction(function=launch_setup),
279+
#Node(
280+
# package="sf000_driver",
281+
# executable="reader.py",
282+
# name="reader",
283+
# remappings=[("/range", "/sf000/range")],
284+
#)
285+
]
286+
)

0 commit comments

Comments
 (0)