Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
Jan Koniarik
schpin-robot
Commits
39ba3c88
Commit
39ba3c88
authored
Oct 10, 2017
by
Jan Koniarik
Browse files
restructured launch files
parent
8b98a784
Changes
1
Hide whitespace changes
Inline
Side-by-side
schpin_tote/src/lib/launch.py
View file @
39ba3c88
...
...
@@ -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
():
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment