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="software@projectmarch.nl">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()