progress changes

schpin_tote/src/lib/config.py

@@ -108,9 +108,11 @@ class Current(Unit):
     def from_ma(cls, val):
         return cls(val / 1000)
 
-    def as_ma(cls, val):
-        return float(val * 1000)
+    def as_ma(self):
+        return float(self * 1000)
 
+    def as_a(self):
+        return float(self)
 
 class Torque(Unit):
     """ torque stored in Nm """

schpin_tote/src/lib/launch.py

@@ -2,12 +2,54 @@
 import yaml
 
 from lxml import etree
-from util import convert_ros, iterate_chain
+from lib.util import convert_ros, iterate_chain
 
 
 def generate_leg_graph_launch(key, leg):
     """ generates leg graph generators launch files for schpin_stg package """
 
+    cfg = {
+        "chain_root": leg['chain_root'],
+        "segments": dict(),
+        "pos": leg['pos'],
+        "rot": leg['rot']
+    }
+
+    for seg_name, seg_cfg in leg['segments'].items():
+
+        seg_cfg = convert_ros( seg_cfg )
+
+        cfg['segments'][seg_name] = {
+            'mass': seg_cfg['mass'],
+            'pos': seg_cfg['pos'],
+            'mass_center': seg_cfg['mass_center'],
+            'rot': seg_cfg['rot'],
+        }
+
+        if 'joint' in seg_cfg:
+            cfg['segments'][seg_name]['joint'] = {
+                'graph': {
+                    'steps': seg_cfg['joint']['graph']['steps']
+                },
+                'angle': {
+                    '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']
+            }
+
+        if 'next_segment' in seg_cfg:
+            cfg['segments'][seg_name]['next_segment'] = seg_cfg['next_segment']
+
     launch = etree.Element('launch')
 
     etree.SubElement(
@@ -18,7 +60,7 @@ def generate_leg_graph_launch(key, leg):
         name='{}_leg_graph_generator'.format(key)
     )
     rosparam = etree.SubElement(launch, 'rosparam')
-    rosparam.text = yaml.dump(leg)
+    rosparam.text = yaml.dump(cfg, default_flow_style=False)
 
     return launch
 
@@ -32,48 +74,88 @@ def generate_robot_description():
         launch,
         "param",
         name="robot_description",
-        textfile="$(find schpin_tote)/src/tote.urdf"
+        textfile="$(find schpin_tote)/urdf/tote.urdf"
     )
 
     return launch
 
 
-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
+def generate_pwm_joints_node(
+    xml_root, name, segments_map
+):
 
-    joint = etree.SubElement(
+    joints = etree.SubElement(
         xml_root,
         "node",
-        pkg="schpin_dev",
-        type="pwm_servo_joint.py",
+        pkg="schpin_control",
+        type="pwm_joints",
         name=name,
-        respawn="true"
+        respawn="true",
+        output="screen"
     )
 
-    params = {
-        'angle_min': segment['joint']['angle']['min'],
-        'angle_max': segment['joint']['angle']['max'],
-        'pulse_min': segment['joint']['pulse']['min'],
-        'pulse_max': segment['joint']['pulse']['max'],
-        "velocity": segment['joint']['max_velocity'],
-        "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': channel_offset + segment['joint']['channel'],
-        'default_post': segment['joint']['default_pos'],
-        'tf_frame': tf_prefix + name
+    controllers = {
+        "joint_state_controller": {
+            "type": "joint_state_controller/JointStateController",
+            "publish_rate": 50
+        },
     }
 
-    for key, val in params.items():
-        etree.SubElement(joint, "param", name=key, value=str(convert_ros(val)))
+    joints_list = []
+
+    for chain in segments_map:
+        for segment_name, segment in chain['segments'].items():
+
+            if 'joint' not in segment:
+                continue
+
+            urdf_name =  chain['name_prefix'] + segment_name
+
+            joints_list.append(urdf_name)
+
+            params = {
+                'angle_min': segment['joint']['angle']['min'],
+                'angle_center': segment['joint']['angle']['center'],
+                'angle_max': segment['joint']['angle']['max'],
+                'pulse_min': segment['joint']['pulse']['min'],
+                'pulse_center': segment['joint']['pulse']['center'],
+                'pulse_max': segment['joint']['pulse']['max'],
+                "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'],
+                '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)
 
+            controllers[urdf_name + "_joint_position_controller"] = {
+                "type": "position_controllers/JointPositionController",
+                "joint": urdf_name
+            }
 
-def generate_dev_layer(cfg):
-    """ generate dev layer of schpin system, meaning all recuired devices AND
+    jointslist_param = etree.SubElement(joints, "rosparam")
+    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)
+
+    etree.SubElement(
+        xml_root,
+        "node",
+        name="controller_spawner",
+        pkg="controller_manager",
+        type="spawner",
+        respawn="false",
+        output="screen",
+        ns=name,
+        args=" ".join(controllers.keys())
+    )
+
+
+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')
@@ -103,41 +185,37 @@ def generate_dev_layer(cfg):
     js_params = etree.SubElement(js_pub, "rosparam")
     js_params.text = yaml.dump(
         {
-            "source_list":
-            ["/dev/leg/" + x + "/joint_states"
-             for x in cfg['legs'].keys()] + ["/dev/gimbal/joint_states"],
+            "source_list": [
+                "/control/leg/" + x + "/joint_states"
+                for x in cfg['legs'].keys()
+            ] + ["/control/gimbal/joint_states"],
             "rate":
             50
         }
+        , default_flow_style=False
     )
 
-    dev_group = etree.SubElement(launch, "group", ns="dev")
+    segments_map = list()
 
-    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(
-                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(
-            xml_root=ggroup, 
-            tf_prefix="cam/", 
-            channel_offset=cfg['gimbal']['channel_offset'],
-            name=name, 
-            segment=segment)
-    )
+        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/"
+       })
+
+    generate_pwm_joints_node(
+xml_root = launch,
+name = 'control',
+segments_map = segments_map
+            )
 
     return launch
 
@@ -147,16 +225,14 @@ def generate_launch_files(cfg):
 
     files = {
         "robot_description.launch": generate_robot_description(),
-        "dev_layer.launch": generate_dev_layer(cfg)
+        "control_layer.launch": generate_control_layer(cfg)
     }
 
     for key in cfg['legs'].keys():
-        files["{}_leg_grpah_generator.launch".format(key)
+        files["{}_leg_graph_generator.launch".format(key)
               ] = generate_leg_graph_launch(key, cfg['legs'][key])
 
     for filename, element in files.items():
         tree = etree.ElementTree(element)
 
         tree.write("../launch/" + filename, pretty_print=True)
-
-

schpin_tote/src/lib/urdf.py

@@ -4,7 +4,7 @@
     """
 import hashlib
 
-from config import Distance, Angle, Unit, Mass
+from lib.config import Distance, Angle, Unit, Mass
 
 from lxml import etree
 
@@ -163,9 +163,9 @@ def generate_urdf(filename, cfg):
         )
 
     generate_urdf_chain(
-        robot, 'cam', cfg['gimbal']['segments'], cfg['gimbal']['chain_root'],
+        robot, 'gimbal', cfg['gimbal']['segments'], cfg['gimbal']['chain_root'],
         base_link
     )
 
     tree = etree.ElementTree(robot)
-    tree.write('tote.urdf', pretty_print=True)
+    tree.write(filename, pretty_print=True)

schpin_tote/src/lib/util.py

-
-from lib.config import Distance, Angle, Unit, AngleVelocity
+from lib.config import Distance, Angle, Unit, AngleVelocity, Mass, Torque, Current
 
 
 def iterate_chain(name, segments, func):
@@ -20,12 +19,23 @@ def convert_ros(val):
         return val.as_radians()
     elif isinstance(val, AngleVelocity):
         return val.as_radians_s()
+    elif isinstance(val, Mass):
+        return val.as_kg()
+    elif isinstance(val, Torque):
+        return val.as_n_m()
+    elif isinstance(val, Current):
+        return val.as_a()
     elif isinstance(val, Unit):
         raise Exception("Don't now how to convert unit for ros", val)
-    elif isinstance(val, (list,tuple)):
+    elif isinstance(val, (list, tuple)):
         res = list()
         for item in val:
-            res.append( convert_ros( item))
+            res.append(convert_ros(item))
+        return res
+    elif isinstance(val, dict):
+        res = dict()
+        for key, item in val.items():
+            res[key] = convert_ros(item )
         return res
     else:
         return val

schpin_tote/src/render

@@ -40,4 +40,4 @@ if args.action == 'launch' or args.action == 'all':
     generate_launch_files(cfg)
 
 if args.action == 'urdf' or args.action == 'all':
-    generate_urdf('tote.urdf', cfg)
+    generate_urdf('../urdf/tote.urdf', cfg,)