Merge pull request #495 from project-march/release/v0.3.0

Release v0.3.0

.travis.yml

@@ -30,6 +30,7 @@ env:
     - BUILDER=colcon
     - CATKIN_LINT=pedantic
     - DOWNSTREAM_WORKSPACE=.rosinstall
+    - AFTER_INSTALL_TARGET_DEPENDENCIES='pip install numpy_ringbuffer'
 
 jobs:
   include:

march_data_collector/CMakeLists.txt

@@ -27,7 +27,7 @@ catkin_package(
     visualization_msgs
 )
 
-catkin_install_python(PROGRAMS scripts/${PROJECT_NAME}
+catkin_install_python(PROGRAMS scripts/${PROJECT_NAME} scripts/esp_adapter
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 install(DIRECTORY launch

march_data_collector/launch/esp_adapter.launch

@@ -0,0 +1,3 @@
+<launch>
+    <node name="esp_adapter" pkg="march_data_collector" type="esp_adapter" output="screen"/>
+</launch>

march_data_collector/launch/march_data_collector.launch

@@ -1,3 +1,17 @@
 <launch>
-    <node name="march_data_collector" pkg="march_data_collector" type="march_data_collector" output="screen"/>
+    <arg name="moticon_ip" default="192.168.8.105" doc="The ip-adress with Moticon software running on it, defaults to
+            EMS switch laptop on standard router"/>
+    <arg name="pressure_soles" default="false" doc="Whether pressure soles will be connected"/>
+    <arg name="esp" default="false" doc="Whether to launch the esp adapter"/>
+    <arg name="logfile" default="false" doc="Whether the data input is from a log file" />
+    <node name="march_data_collector" pkg="march_data_collector" type="march_data_collector" output="screen">
+        <param name="moticon_ip" value="$(arg moticon_ip)" />
+        <param name="pressure_soles" value="$(arg pressure_soles)" />
+    </node>
+    <node if="$(arg esp)" name="esp_adapter" pkg="march_data_collector" type="esp_adapter" output="screen">
+        <param name="logfile" value="$(arg logfile)"/>
+    </node>
+    <group if="$(arg logfile)">
+        <include file="$(find march_launch)/launch/upload_march_urdf.launch" />
+    </group>
 </launch>

march_data_collector/package.xml

@@ -2,7 +2,7 @@
 <package format="2">
   <name>march_data_collector</name>
   <version>0.0.0</version>
-  <description>Collecting data for further processing</description>
+  <description>Collecting data for further processing including sending to an Events Stream Processing engine</description>
   <maintainer email="[email protected]">rutger</maintainer>
   <license>TODO</license>
 

march_data_collector/scripts/esp_adapter

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

march_data_collector/src/march_data_collector/com_calculator.py

@@ -64,7 +64,7 @@ def calculate_com(self):
                 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))
+                rospy.logdebug('TF Error in trying to lookup transform for center of mass: {error}'.format(error=err))
 
         x = x / self.mass
         y = y / self.mass

march_data_collector/src/march_data_collector/cp_calculator.py

@@ -1,6 +1,7 @@
 from math import sqrt
 
 import rospy
+import tf2_ros
 from visualization_msgs.msg import Marker
 
 
@@ -36,16 +37,25 @@ def calculate_cp(self, com_mark):
             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
+            try:
+                trans = self.tf_buffer.lookup_transform('world', self.foot_link, rospy.Time())
+                try:
+                    multiplier = sqrt(com_mark.pose.position.z / self.g)
+                except ValueError:
+                    rospy.logdebug_throttle(1, 'Cannot calculate capture point, because center of mass height is '
+                                               'smaller than 0')
+                    return self.marker
+
+                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
+            except tf2_ros.TransformException as e:
+                rospy.logdebug('Error in trying to lookup transform for capture point: {error}'.format(error=e))
 
         return self.marker
 

march_data_collector/src/march_data_collector/data_collector_node.py

@@ -1,55 +1,94 @@
+import errno
 from math import pi
+import socket
+import sys
 
 from control_msgs.msg import JointTrajectoryControllerState
 from geometry_msgs.msg import TransformStamped
+import numpy
 import rospy
-from sensor_msgs.msg import Imu, Temperature
+from sensor_msgs.msg import Imu
 from tf.transformations import quaternion_from_euler, quaternion_multiply
 import tf2_ros
 from urdf_parser_py.urdf import URDF
 from visualization_msgs.msg import Marker
 
-from march_shared_resources.msg import ImcErrorState
+from march_shared_resources.msg import JointValues, PressureSole
+
 
 from .com_calculator import CoMCalculator
 from .cp_calculator import CPCalculator
 
 
 class DataCollectorNode(object):
-
     def __init__(self, com_calculator, cp_calculators):
+        self.differentiation_order = 2
         self._com_calculator = com_calculator
         self._cp_calculators = cp_calculators
 
-        joint_names = rospy.get_param('/march/joint_names')
+        self.position_memory = []
+        self.time_memory = []
+        self.joint_values = JointValues()
+
+        self.joint_values_publisher = rospy.Publisher('/march/joint_values', JointValues, queue_size=1)
 
         self._imu_broadcaster = tf2_ros.TransformBroadcaster()
         self._com_marker_publisher = rospy.Publisher('/march/com_marker', Marker, queue_size=1)
 
-        self._temperature_subscriber = [rospy.Subscriber('/march/temperature/' + joint,
-                                                         Temperature,
-                                                         self.temperature_callback, joint) for joint in joint_names]
-
         self._trajectory_state_subscriber = rospy.Subscriber('/march/controller/trajectory/state',
                                                              JointTrajectoryControllerState,
                                                              self.trajectory_state_callback)
 
-        self._imc_state_subscriber = rospy.Subscriber('/march/imc_states', ImcErrorState, self.imc_state_callback)
-
         self._imu_subscriber = rospy.Subscriber('/march/imu', Imu, self.imu_callback)
 
-    def temperature_callback(self, data, joint):
-        rospy.logdebug('Temperature' + joint + ' is ' + str(data.temperature))
+        self.pressure_soles_on = rospy.get_param('~pressure_soles')
+        if self.pressure_soles_on:
+            rospy.logdebug('will run with pressure soles')
+            self.output_host = rospy.get_param('~moticon_ip')
+            sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            sock.connect(('8.8.8.8', 53))
+            self.input_host = sock.getsockname()[0]
+            sock.close()
+            self.output_port = 8888
+            self.output_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+            self.input_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            try:
+                self.input_sock.bind((self.input_host, 9999))
+            except socket.error:
+                rospy.logwarn('Cannot connect to host, is the adress correct? \nrunning without pressure soles')
+                self.pressure_soles_on = False
+                self.close_sockets()
+
+            self._pressure_sole_publisher = rospy.Publisher('/march/pressure_soles', PressureSole, queue_size=1)
+        else:
+            rospy.logdebug('running without pressure soles')
 
     def trajectory_state_callback(self, data):
-        rospy.logdebug('received trajectory state' + str(data.desired))
         com = self._com_calculator.calculate_com()
         self._com_marker_publisher.publish(com)
         for cp_calculator in self._cp_calculators:
             cp_calculator.calculate_cp(com)
-
-    def imc_state_callback(self, data):
-        rospy.logdebug('received IMC message current is ' + str(data.current))
+        if self.pressure_soles_on:
+            self.send_udp(data.actual.positions)
+
+        self.position_memory.append(data.actual.positions)
+        self.time_memory.append(data.header.stamp.secs + data.header.stamp.nsecs * 10**(-9))
+        if len(self.position_memory) > self.differentiation_order + 1:
+            self.position_memory.pop(0)
+            self.time_memory.pop(0)
+        if len(self.position_memory) > self.differentiation_order:
+            velocity = numpy.gradient(self.position_memory, self.time_memory, edge_order=self.differentiation_order,
+                                      axis=0)
+            acceleration = numpy.gradient(velocity, self.time_memory, edge_order=self.differentiation_order, axis=0)
+            jerk = numpy.gradient(acceleration, self.time_memory, edge_order=self.differentiation_order, axis=0)
+
+            self.joint_values.controller_output = data
+            self.joint_values.velocities = velocity[-1]
+            self.joint_values.accelerations = acceleration[-1]
+            self.joint_values.jerks = jerk[-1]
+
+            self.joint_values_publisher.publish(self.joint_values)
 
     def imu_callback(self, data):
         if data.header.frame_id == 'imu_link':
@@ -71,16 +110,57 @@ def imu_callback(self, data):
 
             self._imu_broadcaster.sendTransform(transform)
 
+    def send_udp(self, data):
+        message = ' '.join([str(180 * val / pi) for val in data])
+        self.output_sock.sendto(message.encode('utf-8'), (self.output_host, self.output_port))
+
+    def receive_udp(self):
+        try:
+            data, addr = self.input_sock.recvfrom(1024)
+            datachannels = data.split()
+            values = [float(x) for x in datachannels]
+            pressure_sole_msg = PressureSole()
+            pressure_sole_msg.header.stamp = rospy.Time.now()
+            pressure_sole_msg.pressure_soles_time = rospy.Time(values[0])
+            pressure_sole_msg.cop_left = values[1:3]
+            pressure_sole_msg.pressure_left = values[3:19]
+            pressure_sole_msg.total_force_left = values[19]
+            pressure_sole_msg.cop_right = values[20:22]
+            pressure_sole_msg.pressure_right = values[22:38]
+            pressure_sole_msg.total_force_right = values[38]
+            self._pressure_sole_publisher.publish(pressure_sole_msg)
+        except socket.timeout:
+            rospy.loginfo('Has not received pressure sole data in a while, are they on?')
+        except socket.error as error:
+            if error.errno == errno.EINTR:
+                self.close_sockets()
+            else:
+                raise
+        return
+
+    def close_sockets(self):
+        self.output_sock.close()
+        self.input_sock.close()
+
+    def run(self):
+        if self.pressure_soles_on:
+            while not rospy.is_shutdown() and self.pressure_soles_on:
+                self.receive_udp()
+        else:
+            rospy.spin()
+
 
 def main():
     rospy.init_node('data_collector', anonymous=True)
-
-    robot = URDF.from_parameter_server()
+    try:
+        robot = URDF.from_parameter_server('/robot_description')
+    except KeyError:
+        rospy.logerr('Cannot retrieve URDF from parameter server.')
+        sys.exit()
     tf_buffer = tf2_ros.Buffer()
     tf2_ros.TransformListener(tf_buffer)
     center_of_mass_calculator = CoMCalculator(robot, tf_buffer)
     feet = ['ankle_plate_left', 'ankle_plate_right']
     cp_calculators = [CPCalculator(tf_buffer, foot) for foot in feet]
-
-    DataCollectorNode(center_of_mass_calculator, cp_calculators)
-    rospy.spin()
+    data_collector_node = DataCollectorNode(center_of_mass_calculator, cp_calculators)
+    data_collector_node.run()