restructured launch files

schpin_tote/src/lib/launch.py

@@ -17,7 +17,7 @@ def generate_leg_graph_launch(key, leg):
 
     for seg_name, seg_cfg in leg['segments'].items():
 
-        seg_cfg = convert_ros( seg_cfg )
+        seg_cfg = convert_ros(seg_cfg)
 
         cfg['segments'][seg_name] = {
             'mass': seg_cfg['mass'],
@@ -35,16 +35,11 @@ def generate_leg_graph_launch(key, leg):
                     'min': seg_cfg['joint']['angle']['min'],
                     'max': seg_cfg['joint']['angle']['max']
                 },
-                'max_velocity':
-                seg_cfg['joint']['max_velocity'],
-                'max_torque':
-                seg_cfg['joint']['max_torque'],
-                'stall_current':
-                seg_cfg['joint']['stall_current'],
-                'running_current':
-                seg_cfg['joint']['running_current'],
-                'axis':
-                seg_cfg['joint']['axis']
+                'max_velocity': seg_cfg['joint']['max_velocity'],
+                'max_torque': seg_cfg['joint']['max_torque'],
+                'stall_current': seg_cfg['joint']['stall_current'],
+                'running_current': seg_cfg['joint']['running_current'],
+                'axis': seg_cfg['joint']['axis']
             }
 
         if 'next_segment' in seg_cfg:
@@ -80,9 +75,7 @@ def generate_robot_description():
     return launch
 
 
-def generate_pwm_joints_node(
-    xml_root, name, segments_map
-):
+def generate_pwm_joints_node(xml_root, name, segments_map):
 
     joints = etree.SubElement(
         xml_root,
@@ -91,7 +84,7 @@ def generate_pwm_joints_node(
         type="pwm_joints",
         name=name,
         respawn="true",
-        output="screen"
+        machine="robot"
     )
 
     controllers = {
@@ -109,7 +102,7 @@ def generate_pwm_joints_node(
             if 'joint' not in segment:
                 continue
 
-            urdf_name =  chain['name_prefix'] + segment_name
+            urdf_name = chain['name_prefix'] + segment_name
 
             joints_list.append(urdf_name)
 
@@ -123,12 +116,15 @@ def generate_pwm_joints_node(
                 "velocity": segment['joint']['max_velocity'],
                 "update_freq": 50,  # TODO: hardcoded value is far from ideal
                 "reverse": segment['joint']['reverse'],
-                'channel': chain['channel_offset'] + segment['joint']['channel'],
+                'channel':
+                chain['channel_offset'] + segment['joint']['channel'],
                 'default_pos': segment['joint']['default_pos'],
             }
 
             param_element = etree.SubElement(joints, "rosparam", ns=urdf_name)
-            param_element.text = yaml.dump(convert_ros(params), default_flow_style=False)
+            param_element.text = yaml.dump(
+                convert_ros(params), default_flow_style=False
+            )
 
             controllers[urdf_name + "_joint_position_controller"] = {
                 "type": "position_controllers/JointPositionController",
@@ -136,7 +132,11 @@ def generate_pwm_joints_node(
             }
 
     jointslist_param = etree.SubElement(joints, "rosparam")
-    jointslist_param.text = yaml.dump({"joints": joints_list}, default_flow_style=False)
+    jointslist_param.text = yaml.dump(
+        {
+            "joints": joints_list
+        }, default_flow_style=False
+    )
 
     controllers_element = etree.SubElement(joints, "rosparam")
     controllers_element.text = yaml.dump(controllers, default_flow_style=False)
@@ -150,72 +150,145 @@ def generate_pwm_joints_node(
         respawn="false",
         output="screen",
         ns=name,
-        args=" ".join(controllers.keys())
+        args=" ".join(controllers.keys()),
+        machine="robot"
     )
 
 
-def generate_control_layer(cfg):
-    """ generate control layer of schpin system, meaning all recuired devices AND
-        robot_state_publisher/joint_state_publisher combo """
-
-    launch = etree.Element('launch')
+def generate_state_publishers(element, joint_source_list):
 
     etree.SubElement(
-        launch,
-        "include",
-        file="$(find schpin_tote)/launch/robot_description.launch"
-    )
-
-    etree.SubElement(
-        launch,
+        element,
         "node",
         name="robot_state_publisher",
         pkg="robot_state_publisher",
-        type="robot_state_publisher"
+        type="robot_state_publisher",
+        machine="robot"
     )
-
     js_pub = etree.SubElement(
-        launch,
+        element,
         "node",
         name="joint_state_publisher",
         pkg="joint_state_publisher",
         type="joint_state_publisher",
+        machine="robot"
     )
 
     js_params = etree.SubElement(js_pub, "rosparam")
     js_params.text = yaml.dump(
         {
-            "source_list": [
-                "/control/leg/" + x + "/joint_states"
-                for x in cfg['legs'].keys()
-            ] + ["/control/gimbal/joint_states"],
-            "rate":
-            50
-        }
-        , default_flow_style=False
+            "source_list": joint_source_list,
+            "rate": 50
+        },
+        default_flow_style=False
+    )
+
+
+def generate_machines(element):
+    etree.SubElement(
+        element, "machine", name="master", address="$(env MASTER_ADDRESS)"
+    )
+
+    robot = etree.SubElement(
+        element,
+        "machine",
+        name="robot",
+        address="$(env ROBOT_ADDRESS)",
+        default="true",
+        user="$(env ROBOT_USER)"
+    )
+    robot.set("env-loader", "~/setup.bash")
+
+
+def generate_control_layer(cfg):
+    """ generate control layer of schpin system, meaning all recuired devices AND
+        robot_state_publisher/joint_state_publisher combo """
+
+    launch = etree.Element('launch')
+
+    generate_state_publishers(
+        launch,
+        ["/control/leg/" + x + "/joint_states"
+         for x in cfg['legs'].keys()] + ["/control/gimbal/joint_states"]
     )
 
     segments_map = list()
 
     for key, l_cfg in cfg['legs'].items():
 
-        segments_map.append({
-            "channel_offset": l_cfg["channel_offset"],
-            "segments": l_cfg['segments'],
-            "name_prefix": "leg/" + key + "/"
-            })
+        segments_map.append(
+            {
+                "channel_offset": l_cfg["channel_offset"],
+                "segments": l_cfg['segments'],
+                "name_prefix": "leg/" + key + "/"
+            }
+        )
 
-    segments_map.append({
-       "channel_offset" : cfg['gimbal']['channel_offset'],
-       "segments": cfg['gimbal']['segments'],
-       "name_prefix": "gimbal/"
-       })
+    segments_map.append(
+        {
+            "channel_offset": cfg['gimbal']['channel_offset'],
+            "segments": cfg['gimbal']['segments'],
+            "name_prefix": "gimbal/"
+        }
+    )
 
     generate_pwm_joints_node(
-xml_root = launch,
-name = 'control',
-segments_map = segments_map
-            )
+        xml_root=launch, name='control', segments_map=segments_map
+    )
+
+    return launch
+
+
+def generate_general(cfg):
+
+    launch = etree.Element('launch')
+
+    generate_machines(launch)
+
+    etree.SubElement(
+        launch,
+        "include",
+        file="$(find schpin_tote)/launch/robot_description.launch"
+    )
+
+    return launch
+
+
+def generate_user_layer(cfg):
+
+    launch = etree.Element('launch')
+
+    etree.SubElement(
+        launch,
+        "node",
+        name="$(anon rviz)",
+        pkg="rviz",
+        type="rviz",
+        machine="master"
+    )
+
+    etree.SubElement(
+        launch,
+        "node",
+        name="$(anon rqt)",
+        pkg="rqt",
+        type="rqt",
+        machine="master"
+    )
+    return launch
+
+
+def generate_main(cfg):
+    launch = etree.Element('launch')
+
+    to_include = [
+        "general.launch", "control_layer.launch", "user_layer.launch"
+    ]
+
+    for filename in to_include:
+        etree.SubElement(
+            launch, "include", file="$(find schpin_tote)/launch/" + filename
+        )
 
     return launch
 
@@ -225,7 +298,10 @@ def generate_launch_files(cfg):
 
     files = {
         "robot_description.launch": generate_robot_description(),
-        "control_layer.launch": generate_control_layer(cfg)
+        "control_layer.launch": generate_control_layer(cfg),
+        "user_layer.launch": generate_user_layer(cfg),
+        "general.launch": generate_general(cfg),
+        "main.launch": generate_main(cfg),
     }
 
     for key in cfg['legs'].keys():