Merge branch 'develop'

.gitignore

@@ -32,6 +32,7 @@ docs/build/*
 # qcreator stuff
 CMakeLists.txt.user
 
+*.egg-info/
 srv/_*.py
 *.pcd
 *.pyc
@@ -51,6 +52,7 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
+COLCON_IGNORE
 
 install/*
 

.travis.yml

@@ -29,9 +29,11 @@ env:
     - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci
     - BUILDER=colcon
     - CATKIN_LINT=pedantic
+    - CATKIN_LINT_ARGS='--ignore launch_depend'
     # DEBIAN_FRONTEND disables interactive frontends when installing packages. This is necessary for tzdata.
     - DOCKER_RUN_OPTS='-e DEBIAN_FRONTEND=noninteractive'
     - DOWNSTREAM_WORKSPACE=.rosinstall
+    - NOT_TEST_DOWNSTREAM=true
     - AFTER_INSTALL_TARGET_DEPENDENCIES='pip install numpy_ringbuffer'
 
 jobs:

march_data_collector/CMakeLists.txt

@@ -1,16 +1,13 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(march_data_collector)
 
 find_package(catkin REQUIRED COMPONENTS
     control_msgs
     geometry_msgs
     march_shared_resources
-    rospy
     sensor_msgs
     std_msgs
-    tf
     tf2_geometry_msgs
-    tf2_ros
     visualization_msgs
 )
 
@@ -20,7 +17,7 @@ catkin_package(
     CATKIN_DEPENDS
     control_msgs
     geometry_msgs
-    rospy
+    march_shared_resources
     sensor_msgs
     std_msgs
     tf2_geometry_msgs

march_data_collector/package.xml

@@ -1,22 +1,24 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>march_data_collector</name>
   <version>0.0.0</version>
   <description>Collecting data for further processing including sending to an Events Stream Processing engine</description>
   <maintainer email="[email protected]">rutger</maintainer>
   <license>TODO</license>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>python3-setuptools</buildtool_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>march_shared_resources</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>tf</exec_depend>
 </package>

march_data_collector/setup.py

@@ -1,10 +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
+from setuptools import setup
 
-# fetch values from package.xml
 setup_args = generate_distutils_setup(
     packages=['march_data_collector'],
     package_dir={'': 'src'},

march_data_collector/src/march_data_collector/esp_adapter.py

@@ -51,7 +51,9 @@ def __init__(self):
         self.esp_publishers = {}
         self.ros_subscribers = {}
 
-        self.control_analysis_join_frequency = 250
+        self.previous_join_key = {}
+
+        self.control_analysis_join_frequency = 50
 
         basic_url = 'dfESP://localhost:9901'
         self.project = basic_url + '/project_march'
@@ -156,6 +158,7 @@ def configure_source(self, sources, topic, msg_type, callback):
             self.esp_publishers[source] = (pub, schemaptr)
             rospy.logdebug('configured ESP source window for ' + source)
             self.ros_subscribers[source] = sub
+        self.previous_join_key[sources[0]] = None
 
     def send_to_esp(self, csv, sources):
         """Sends a csv string to the configured source window. Also adds standard stuff to the start of the csv string.
@@ -215,6 +218,11 @@ def joint_command_callback(self, data, sources):
         """
         time_str = get_time_str(data.header.stamp)
         join_time_str = get_join_time_str(data.header.stamp, self.control_analysis_join_frequency)
+
+        if self.previous_join_key[sources[0]] == join_time_str:
+            return
+        self.previous_join_key[sources[0]] = join_time_str
+
         csv = list_to_str(['1', join_time_str, data.header.seq, time_str, list_to_str(data.effort_command)])
         self.send_to_esp(csv, sources)
 
@@ -271,23 +279,37 @@ def pid_state_callback(self, data, sources):
         """
         time_str = get_time_str(data.header.stamp)
         join_time_str = get_join_time_str(data.header.stamp, self.control_analysis_join_frequency)
+
+        if self.previous_join_key[sources[0]] == join_time_str:
+            return
+        self.previous_join_key[sources[0]] = join_time_str
+
         csv = list_to_str([data.p_error, data.i_error, data.d_error, data.p_term, data.i_term,
                           data.d_term, data.output])
-        self.send_to_esp('{0}, {1}, {2}'.format(time_str, join_time_str, csv), sources)
+        self.send_to_esp('{0}, {1}, {2}, 1'.format(time_str, join_time_str, csv), sources)
 
     def imc_state_callback(self, data, sources):
         """Callback for IMotionCube data. Converts ROS message to csv string to send to the source window.
 
         :param data: ROS march_shared_resources.msgs.ImcErrorState message
         :param sources: list of source windows in the ESP engine
         """
+        time_str = get_time_str(data.header.stamp)
+        join_time_str = get_join_time_str(data.header.stamp, self.control_analysis_join_frequency)
+
+        if self.previous_join_key[sources[0]] == join_time_str:
+            return
+        self.previous_join_key[sources[0]] = join_time_str
+
         motor_current_str = ','.join([str(value) for value in data.motor_current])
         imc_voltage_str = ','.join([str(value) for value in data.imc_voltage])
         absolute_encoder_str = ','.join([str(value) for value in data.absolute_encoder_value])
         incremental_encoder_str = ','.join([str(value) for value in data.incremental_encoder_value])
-        time_str = get_time_str(data.header.stamp)
-        join_time_str = get_join_time_str(data.header.stamp, self.control_analysis_join_frequency)
-        csv = ','.join([time_str, imc_voltage_str, motor_current_str, absolute_encoder_str, incremental_encoder_str])
+        motor_voltage_str = ','.join([str(value) for value in data.motor_voltage])
+        absolute_velocity_str = ','.join([str(value) for value in data.absolute_velocity])
+        incremental_velocity_str = ','.join([str(value) for value in data.incremental_velocity])
+        csv = ','.join([time_str, motor_current_str, imc_voltage_str, motor_voltage_str, absolute_encoder_str,
+                        incremental_encoder_str, absolute_velocity_str, incremental_velocity_str])
         self.send_to_esp('{0}, {1}, 1, {2}'.format(join_time_str, data.header.seq, csv), sources)
 
     def gait_callback(self, data, sources):
@@ -296,8 +318,8 @@ def gait_callback(self, data, sources):
         :param data: ROS march_shared_resoruces.GaitActionGoal message
         :param sources: list of source windows in the ESP engine
         """
-        csv = list_to_str(['1, 1', get_time_str(data.header.stamp), data.goal.name, data.goal.current_subgait.name,
-                          data.goal.current_subgait.version, data.goal.current_subgait.gait_type])
+        csv = list_to_str(['1, 1', get_time_str(data.header.stamp), data.goal.gait_name, data.goal.subgait_name,
+                          data.goal.version, data.goal.gait_type])
         self.send_to_esp(csv, sources)
 
     def com_callback(self, data, sources):
@@ -330,7 +352,7 @@ def get_time_str(timestamp):
 
     :param data: ROS timestamp message std_msgs/stamp
     """
-    time = round(timestamp.secs + timestamp.nsecs * 10 ** (-9), 3)
+    time = round(timestamp.to_sec(), 3)
     return datetime.datetime.fromtimestamp(time).strftime('%Y-%m-%d %H:%M:%S.%f')
 
 
@@ -339,7 +361,7 @@ def get_join_time_str(timestamp, frequency):
 
     :param data: ROS timestamp message std_msgs/stamp
     """
-    time = round(timestamp.secs + timestamp.nsecs * 10 ** (-9) * frequency) / frequency
+    time = round(timestamp.to_sec() * frequency) / frequency
     return datetime.datetime.fromtimestamp(time).strftime('%Y-%m-%d %H:%M:%S.%f')
 
 

march_description/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(march_description)
 
 find_package(catkin REQUIRED COMPONENTS xacro)

march_description/package.xml

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
     <name>march_description</name>
     <version>0.0.0</version>
     <description>The exoskeleton 3D model</description>