Merge branch 'release-v0.1.0'

.flake8

@@ -0,0 +1,40 @@
+[flake8]
+show-source = true
+max-line-length = 120
+format = ${cyan}%(path)s${reset}:${yellow_bold}%(row)d${reset}:${green_bold}%(col)d${reset}: ${red_bold}%(code)s${reset} %(text)s
+exclude = .git
+application-package-names =
+    march_description,
+    march_gain_scheduling,
+    march_gait_scheduler,
+    march_gait_selection,
+    march_launch,
+    march_safety,
+    march_shared_classes,
+    march_shared_resources,
+    march_sound_scheduler,
+    march_state_machine
+import-order-style = appnexus
+ignore =
+    # D100: Missing docstring in public module
+    D100,
+    # D102: Missing docstring in public class
+    D101,
+    # D102: Missing docstring in public method
+    D102,
+    # D103: Missing docstring in public function
+    D103,
+    # D104: Missing docstring in public package
+    D104,
+    # D105: Missing docstring in magic method
+    D105,
+    # D106: Missing docstring in public nested class
+    D106,
+    # D107: Missing docstring in __init__
+    D107,
+    # D401: First line should be in imperative mood
+    D401,
+    # W503: Line break before operator
+    W503,
+    # C815: Trailing comma in Python 3.5
+    C815

.travis.yml

@@ -29,17 +29,20 @@ env:
     - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci
     - BUILDER=colcon
     - CATKIN_LINT=pedantic
-    - DOWNSTREAM_WORKSPACE=dependencies.rosinstall
-    - AFTER_BUILD_TARGET_WORKSPACE='builder_run_build "$target_ws/install" "$target_ws" --cmake-target-skip-unavailable --cmake-target roslint'
+    - DOWNSTREAM_WORKSPACE=.rosinstall
 
 jobs:
   include:
-    - name: "Kinetic"
-      env: ROS_DISTRO=kinetic
     - name: "Melodic"
       env: ROS_DISTRO=melodic
     - name: "clang-format"
       env: ROS_DISTRO=melodic CLANG_FORMAT_CHECK=file
+    - name: "flake8"
+      language: python
+      python: 2.7
+      install:
+        - pip install flake8 pep8-naming flake8-blind-except flake8-string-format flake8-builtins flake8-commas flake8-quotes flake8-print flake8-docstrings flake8-import-order flake8-colors
+      script: flake8
 
 # clone and run industrial_ci
 install:

CODEOWNERS

@@ -0,0 +1,4 @@
+* @Olavhaasie @project-march/v-software
+
+/march_data_collector/ @RutgerVanBeek @project-march/v-software @Olavhaasie
+/march_gain_scheduling/ @project-march/v-control @project-march/v-software @Olavhaasie

README.md

@@ -1,6 +1,8 @@
 # March
 The main repository of the MARCH exoskeleton.
 
+![GitHub release (latest by date including pre-releases)](https://img.shields.io/github/v/release/project-march/march?include_prereleases)
+
 | Branch | Build Status |
 | ------ |:------------:|
 | master | [![Build Status](https://api.travis-ci.com/project-march/march.svg?branch=master)](https://travis-ci.com/project-march/march) |

march_data_collector/CMakeLists.txt

@@ -3,26 +3,30 @@ project(march_data_collector)
 
 find_package(catkin REQUIRED COMPONENTS
     control_msgs
+    geometry_msgs
     march_shared_resources
-    roslint
     rospy
     sensor_msgs
     std_msgs
+    tf
+    tf2_geometry_msgs
+    tf2_ros
+    visualization_msgs
 )
 
 catkin_python_setup()
 
 catkin_package(
     CATKIN_DEPENDS
     control_msgs
+    geometry_msgs
     rospy
     sensor_msgs
     std_msgs
+    tf2_geometry_msgs
+    visualization_msgs
 )
 
-file(GLOB_RECURSE lintfiles "src/*.py")
-roslint_python(${lintfiles})
-
 catkin_install_python(PROGRAMS scripts/${PROJECT_NAME}
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

march_data_collector/package.xml

@@ -8,12 +8,15 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>roslint</build_depend>
-
   <depend>rospy</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>control_msgs</depend>
   <depend>march_shared_resources</depend>
+  <depend>tf</depend>
+  <depend>tf2_ros</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
 
 </package>

march_data_collector/scripts/march_data_collector

@@ -1,6 +1,5 @@
 #!/usr/bin/env python
-
-from march_data_collector import march_data_collector_node
+from march_data_collector import data_collector_node
 
 if __name__ == '__main__':
-    march_data_collector_node.main()
+    data_collector_node.main()

march_data_collector/setup.py

@@ -1,6 +1,7 @@
 # ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
 
 from distutils.core import setup
+
 from catkin_pkg.python_setup import generate_distutils_setup
 
 # fetch values from package.xml
@@ -10,4 +11,4 @@
     scripts=['scripts/march_data_collector'],
 )
 
-setup(**setup_args)
+setup(**setup_args)

march_data_collector/src/march_data_collector/com_calculator.py

@@ -0,0 +1,80 @@
+# Copyright (c) Hamburg Bit-Bots
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy of
+# this software and associated documentation files (the 'Software'), to deal in
+# the Software without restriction, including without limitation the rights to
+# use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+# of the Software, and to permit persons to whom the Software is furnished to do
+# so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+import geometry_msgs.msg
+import rospy
+import tf2_geometry_msgs as tf_geo
+import tf2_ros
+from visualization_msgs.msg import Marker
+
+
+class CoMCalculator(object):
+
+    def __init__(self, robot, tf_buffer):
+        self.tf_buffer = tf_buffer
+
+        self.links = dict(filter(lambda (_, l): l.inertial is not None, robot.link_map.items()))
+        self.mass = sum(l.inertial.mass for (_, l) in self.links.items())
+
+        self.marker = Marker()
+        self.marker.header.frame_id = 'world'
+        self.marker.type = self.marker.SPHERE
+        self.marker.action = self.marker.ADD
+        self.marker.pose.orientation.w = 1.0
+        self.marker.color.a = 1.0
+        self.marker.color.r = 1.0
+        self.marker.scale.x = 0.03
+        self.marker.scale.y = 0.03
+        self.marker.scale.z = 0.03
+
+    def calculate_com(self):
+        x = 0
+        y = 0
+        z = 0
+        for link in self.links:
+            try:
+                trans = self.tf_buffer.lookup_transform('world', link, rospy.Time())
+
+                to_transform = geometry_msgs.msg.PointStamped()
+                to_transform.point.x = self.links[link].inertial.origin.xyz[0]
+                to_transform.point.y = self.links[link].inertial.origin.xyz[1]
+                to_transform.point.z = self.links[link].inertial.origin.xyz[2]
+                to_transform.header.frame_id = link
+                to_transform.header.stamp = rospy.get_rostime()
+                transformed = tf_geo.do_transform_point(to_transform, trans)
+
+                # calculate part of CoM equation depending on link
+                x += self.links[link].inertial.mass * transformed.point.x
+                y += self.links[link].inertial.mass * transformed.point.y
+                z += self.links[link].inertial.mass * transformed.point.z
+            except tf2_ros.TransformException as err:
+                rospy.logwarn('error in CoM calculation' + str(err))
+
+        x = x / self.mass
+        y = y / self.mass
+        z = z / self.mass
+
+        # send CoM position to RViZ
+        self.marker.header.stamp = rospy.get_rostime()
+        self.marker.pose.position.x = x
+        self.marker.pose.position.y = y
+        self.marker.pose.position.z = z
+        rospy.logdebug('center of mass is at ' + str(self.marker.pose.position))
+
+        return self.marker

march_data_collector/src/march_data_collector/cp_calculator.py

@@ -0,0 +1,60 @@
+from math import sqrt
+
+import rospy
+from visualization_msgs.msg import Marker
+
+
+class CPCalculator(object):
+
+    def __init__(self, tf_buffer, foot_link):
+        self.tf_buffer = tf_buffer
+        self.foot_link = foot_link
+        self.publisher = rospy.Publisher('/march/cp_marker_' + foot_link, Marker, queue_size=1)
+
+        self.prev_x = 0
+        self.prev_y = 0
+        self.prev_t = rospy.Time.now()
+
+        self.marker = Marker()
+
+        self.marker.header.frame_id = 'world'
+        self.marker.type = self.marker.SPHERE
+        self.marker.action = self.marker.ADD
+        self.marker.pose.orientation.w = 1.0
+        self.marker.color.a = 1.0
+        self.marker.color.g = 1.0
+        self.marker.scale.x = 0.03
+        self.marker.scale.y = 0.03
+        self.marker.scale.z = 0.03
+
+        self.g = 9.81  # gravity constant
+
+    def calculate_cp(self, com_mark):
+        current_time = com_mark.header.stamp
+        time_difference = (current_time - self.prev_t).to_sec()
+        if current_time is not self.prev_t:
+            x_dot = (com_mark.pose.position.x - self.prev_x) / time_difference
+            y_dot = (com_mark.pose.position.y - self.prev_y) / time_difference
+
+            trans = self.tf_buffer.lookup_transform('world', self.foot_link, rospy.Time())
+            multiplier = sqrt(com_mark.pose.position.z / self.g)
+            x_cp = trans.transform.translation.x + x_dot * multiplier
+            y_cp = trans.transform.translation.y + y_dot * multiplier
+
+            self.update_marker(x_cp, y_cp)
+
+            self.prev_x = com_mark.pose.position.x
+            self.prev_y = com_mark.pose.position.y
+            self.prev_t = current_time
+
+        return self.marker
+
+    def update_marker(self, x_cp, y_cp):
+        self.marker.header.stamp = rospy.get_rostime()
+        self.marker.pose.position.x = x_cp
+        self.marker.pose.position.y = y_cp
+        self.marker.pose.position.z = 0
+
+        rospy.logdebug('capture point is at ' + str(self.marker.pose.position))
+
+        self.publisher.publish(self.marker)