Commit 644863c0 authored by Jan Koniarik's avatar Jan Koniarik
Browse files

code review of launch generation, repaired channel generation

parent 2ae15a1c
......@@ -36,6 +36,7 @@ mg90s: &MG90S
angle:
min: -75°
max: 75°
default_pos: 0
pulse:
min: 900
max: 2100
......@@ -81,6 +82,7 @@ gimbal:
chain_root: 'yaw'
channel_offset: 12
segments:
yaw:
pos:
......@@ -95,6 +97,7 @@ gimbal:
<<: *MG90S
axis: [0,0,1]
reverse: False
channel: 0
next_segment: 'pitch'
pitch:
pos:
......@@ -106,6 +109,7 @@ gimbal:
<<: *MG90S
axis: [0, 1, 0]
reverse: False
channel: 1
scad:
module: 'camera_pitch'
args: []
......@@ -114,9 +118,11 @@ battery:
offset: [-5mm,0,0]
dim: [80mm,12mm,35mm]
default_leg: &DEFAULT_LEG
mirror: [0,0,0]
chain_root: coxa
channel_offset: 0
segments: # min( torque_needed / max_torque , 1 ) * ( stall_current - running_current ) + stall_current
coxa:
pos: [0,0,0]
......@@ -127,6 +133,7 @@ default_leg: &DEFAULT_LEG
graph:
steps: 16
reverse: False
channel: 0
mass: 30g
mass_center: [0,10mm,-5mm]
scad:
......@@ -145,6 +152,7 @@ default_leg: &DEFAULT_LEG
graph:
steps: 16
reverse: False
channel: 1
mass: 10g
mass_center: [0, 25mm ,0]
scad:
......@@ -160,6 +168,7 @@ default_leg: &DEFAULT_LEG
graph:
steps: 16
reverse: False
channel: 2
mass: 15g
mass_center: [0,25mm,0]
scad:
......@@ -177,20 +186,23 @@ legs:
mirror: [1,0,0]
pos: [55mm, 25mm, 0]
rot: [0,0,-30°]
channel_offset: 0
fr:
<<: *DEFAULT_LEG
pos: [55mm, -25mm, 0]
rot: [0,0,-150°]
channel_offset: 3
rl:
<<: *DEFAULT_LEG
pos: [-55mm, 25mm, 0]
rot: [0,0,30°]
channel_offset: 6
rr:
<<: *DEFAULT_LEG
mirror: [1,0,0]
pos: [-55mm, -25mm, 0]
rot: [0,0,150°]
channel_offset: 9
rpi:
holes:
......
......@@ -233,7 +233,7 @@ def _parse_val(val, top_cfg):
# ignored_names is set of strings that are not considered variables
ignored_names = ["max", "min", "abs"]
for unit_type in types:
ignored_names.append(str(unit_type))
ignored_names.append(str(unit_type.__name__))
# for each matched formula:
for var in vars:
......
......@@ -2,10 +2,11 @@
import yaml
from lxml import etree
from util import convert_ros
from util import convert_ros, iterate_chain
def generate_leg_graph_launch(key, leg):
""" generates leg graph generators launch files for schpin_stg package """
launch = etree.Element('launch')
......@@ -23,6 +24,7 @@ def generate_leg_graph_launch(key, leg):
def generate_robot_description():
""" genereates launch file for setting robot description from this package """
launch = etree.Element("launch")
......@@ -36,10 +38,12 @@ def generate_robot_description():
return launch
# TODO: lxml should be dependency, so every other module, maybe grep-check this?
def generate_servo_pwm_node(xml_root, tf_prefix, name, segment):
def generate_servo_pwm_node(xml_root, tf_prefix, channel_offset, name, segment):
""" generates node for pwm_servo_joint based on segment configuration """
if 'joint' not in segment:
return
joint = etree.SubElement(
xml_root,
"node",
......@@ -55,12 +59,12 @@ def generate_servo_pwm_node(xml_root, tf_prefix, name, segment):
'pulse_min': segment['joint']['pulse']['min'],
'pulse_max': segment['joint']['pulse']['max'],
"velocity": segment['joint']['max_velocity'],
"update_freq": 50, # TODO: fix this
"update_freq": 50, # TODO: hardcoded value is far from ideal
"reverse": segment['joint']['reverse'],
"controller_topic": "/hw/controller_topic",
"joint_state_topic": "joint_states",
'channel': 0,
'default_post': 0,
'channel': channel_offset + segment['joint']['channel'],
'default_post': segment['joint']['default_pos'],
'tf_frame': tf_prefix + name
}
......@@ -69,6 +73,8 @@ def generate_servo_pwm_node(xml_root, tf_prefix, name, segment):
def generate_dev_layer(cfg):
""" generate dev layer of schpin system, meaning all recuired devices AND
robot_state_publisher/joint_state_publisher combo """
launch = etree.Element('launch')
......@@ -98,38 +104,46 @@ def generate_dev_layer(cfg):
js_params.text = yaml.dump(
{
"source_list":
["/dev/leg/" + x + "/joint_states" for x in cfg['legs'].keys()] +
["/dev/gimbal/joint_states"]
["/dev/leg/" + x + "/joint_states"
for x in cfg['legs'].keys()] + ["/dev/gimbal/joint_states"],
"rate":
50
}
)
dev_group = etree.SubElement(launch, "group", ns="dev")
leg_group = etree.SubElement(dev_group, "group", ns="leg")
for key, l_cfg in cfg['legs'].items():
lgroup = etree.SubElement(leg_group, "group", ns=key)
iterate_chain( l_cfg['chain_root'], l_cfg['segments'], lambda name, segment: generate_servo_pwm_node(lgroup, "leg/" + key + "/", name , segment))
iterate_chain(
l_cfg['chain_root'],
l_cfg['segments'],
lambda name, segment: generate_servo_pwm_node(
xml_root = lgroup,
tf_prefix = "leg/" + key + "/",
channel_offset = l_cfg['channel_offset'],
name = name ,
segment = segment)
)
ggroup = etree.SubElement(dev_group, "group", ns="gimbal")
iterate_chain(
cfg['gimbal']['chain_root'], cfg['gimbal']['segments'],
lambda name, segment: generate_servo_pwm_node(ggroup, "cam/", name, segment)
lambda name, segment: generate_servo_pwm_node(
xml_root=ggroup,
tf_prefix="cam/",
channel_offset=cfg['gimbal']['channel_offset'],
name=name,
segment=segment)
)
return launch
def iterate_chain(name, segments, func):
#TODO: use this elsewhere
func(name, segments[name])
if 'next_segment' in segments[name]:
iterate_chain(segments[name]['next_segment'], segments, func)
def generate_launch_files(cfg):
""" generates all required launch files """
files = {
"robot_description.launch": generate_robot_description(),
......@@ -144,3 +158,5 @@ def generate_launch_files(cfg):
tree = etree.ElementTree(element)
tree.write("../launch/" + filename, pretty_print=True)
from lib.config import Distance, Angle, Unit, AngleVelocity
def convert_ros(val):
def iterate_chain(name, segments, func):
""" Executes func(name, segment_config) based on 'segments' and 'name',
than recursively calls itself on
segment_config['next_segment'] if it exists. """
func(name, segments[name])
if 'next_segment' in segments[name]:
iterate_chain(segments[name]['next_segment'], segments, func)
def convert_ros(val):
""" converts data into ros compatible units """
if isinstance(val, Distance):
return val.as_m()
elif isinstance(val, Angle):
......
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