Commit 39ba3c88 authored by Jan Koniarik's avatar Jan Koniarik
Browse files

restructured launch files

parent 8b98a784
......@@ -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():
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment