13
13
# limitations under the License.
14
14
15
15
import launch
16
+ from launch .actions import DeclareLaunchArgument
17
+ from launch .conditions import IfCondition
18
+ from launch .substitutions import LaunchConfiguration
16
19
from launch_ros .actions import ComposableNodeContainer
20
+ from launch_ros .actions import LoadComposableNodes
17
21
from launch_ros .descriptions import ComposableNode
18
22
19
23
@@ -28,7 +32,17 @@ def _create_api_node(node_name, class_name, **kwargs):
28
32
29
33
30
34
def generate_launch_description ():
31
- components = [
35
+ launch_arguments = []
36
+
37
+ def add_launch_arg (name : str , default_value = None , description = None ):
38
+ launch_arguments .append (
39
+ DeclareLaunchArgument (name , default_value = default_value , description = description )
40
+ )
41
+
42
+ add_launch_arg ("launch_calibration_status_api" , None , "launch calibration status api" )
43
+ add_launch_arg ("launch_start_api" , None , "launch start api" )
44
+
45
+ default_components = [
32
46
_create_api_node ("cpu_usage" , "CpuUsage" ),
33
47
_create_api_node ("diagnostics" , "Diagnostics" ),
34
48
_create_api_node ("door" , "Door" ),
@@ -41,7 +55,6 @@ def generate_launch_description():
41
55
_create_api_node ("metadata_packages" , "MetadataPackages" ),
42
56
_create_api_node ("route" , "Route" ),
43
57
_create_api_node ("service" , "Service" ),
44
- _create_api_node ("start" , "Start" ),
45
58
_create_api_node ("vehicle_status" , "VehicleStatus" ),
46
59
_create_api_node ("velocity" , "Velocity" ),
47
60
_create_api_node ("version" , "Version" ),
@@ -51,7 +64,22 @@ def generate_launch_description():
51
64
name = "autoware_iv_adaptor" ,
52
65
package = "rclcpp_components" ,
53
66
executable = "component_container_mt" ,
54
- composable_node_descriptions = components ,
67
+ composable_node_descriptions = default_components ,
55
68
output = "screen" ,
56
69
)
57
- return launch .LaunchDescription ([container ])
70
+
71
+ calibration_status_loader = LoadComposableNodes (
72
+ composable_node_descriptions = [_create_api_node ("calibration_status" , "CalibrationStatus" )],
73
+ target_container = container ,
74
+ condition = IfCondition (LaunchConfiguration ("launch_calibration_status_api" )),
75
+ )
76
+
77
+ start_loader = LoadComposableNodes (
78
+ composable_node_descriptions = [_create_api_node ("start" , "Start" )],
79
+ target_container = container ,
80
+ condition = IfCondition (LaunchConfiguration ("launch_start_api" )),
81
+ )
82
+
83
+ return launch .LaunchDescription (
84
+ launch_arguments + [container , calibration_status_loader , start_loader ]
85
+ )
0 commit comments