Dynamic filters

.travis.rosinstall

@@ -1,7 +1,7 @@
 - git:
     uri: 'https://github.com/iirob/iirob_filters.git'
     local-name: iirob_filters
-    version: kinetic-devel
+    version: raw_with_fts
 - git:
     uri: 'https://github.com/ipa320/cob_driver.git'
     local-name: cob_driver

CMakeLists.txt

@@ -21,6 +21,8 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   trajectory_msgs
   visualization_msgs
+  realtime_tools
+  filters
 )
 
 find_package(Eigen3)
@@ -66,7 +68,7 @@ generate_messages(
 ###################################
 catkin_package(
  INCLUDE_DIRS common/include ros/include
- CATKIN_DEPENDS cob_generic_can geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy tf2 tf2_ros tf2_geometry_msgs trajectory_msgs
+ CATKIN_DEPENDS cob_generic_can geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy tf2 tf2_ros tf2_geometry_msgs trajectory_msgs realtime_tools filters
  DEPENDS Eigen
  LIBRARIES ${PROJECT_NAME}
 )

cfg/PublishConfiguration.params

@@ -8,5 +8,6 @@ gen.add("moving_mean", paramtype="bool", description="publish moving mean filter
 gen.add("transformed_data", paramtype="bool", description="publish transformed data", default=True, configurable=True)
 gen.add("gravity_compensated", paramtype="bool", description="publish gravity compensated data", default=True, configurable=True)
 gen.add("threshold_filtered", paramtype="bool", description="publish threshold filtered data", default=True, configurable=True)
+gen.add("iirob_led", paramtype="bool", description="publish data for iirob_led", default=True, configurable=True)
 
 exit(gen.generate("ati_force_torque", "ForceTorqueSensor", "PublishConfiguration"))

config/sensor_configuration.yaml

@@ -10,63 +10,32 @@ Calibration:
         n_measurements: 500
         T_between_meas: 0.01
         isStatic: false # true= use static offset Calibration, false=use calibrate Sensor to 0 in current State
-
+        
 ThresholdFilter:
-  linear_threshold: 2.5
-  angular_threshold: 0.3
+  name: ThresholdFilter
+  type: iirob_filters/ThresholdFilterWrench
+  params: {linear_threshold: 2.5,  angular_threshold: 0.3}
 
 LowPassFilter:
-    Force_x:
-        SamplingFrequency: 200.0
-        DampingFrequency: 15.0
-        DampingIntensity: -6.0
-    Force_y:
-        SamplingFrequency: 200.0
-        DampingFrequency: 15.0
-        DampingIntensity: -6.0
-    Force_z:
-        SamplingFrequency: 200.0
-        DampingFrequency: 15.0
-        DampingIntensity: -6.0
-    Torque_x:
-        SamplingFrequency: 200.0
-        DampingFrequency: 15.0
-        DampingIntensity: -6.0
-    Torque_y:
-        SamplingFrequency: 200.0
-        DampingFrequency: 15.0
-        DampingIntensity: -6.0
-    Torque_z:
-        SamplingFrequency: 200.0
-        DampingFrequency: 15.0
-        DampingIntensity: -6.0
-
+  name: LowPassFilter
+  type: iirob_filters/LowPassFilterWrench
+  params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0}
+
 MovingMeanFilter:
-    Force_x:
-        divider: 4
-    Force_y:
-        divider: 4
-    Force_z:
-        divider: 4
-    Torque_x:
-        divider: 4
-    Torque_y:
-        divider: 4
-    Torque_z:
-        divider: 4
+  name: MovingMeanFilter
+  type: iirob_filters/MovingMeanFilterWrench
+  params: {divider: 4}
 
 GravityCompensation:
-    #CoG:
-     #x: 0
-     #y: 0
-     #z: 0.027346102 # m
-    #force: 7.9459955 # f_G
-    world_frame: "world"
-
+  name: GravityCompensation
+  type: iirob_filters/GravityCompensatorWrench
+  params: {world_frame: "base_link"}
+
 Publish:
     sensor_data: true
     low_pass: true
     moving_mean: true
     transformed_data: true
     gravity_compensated: true
     threshold_filtered: true
+

description/urdf/macros/mini58_sim.urdf.xacro

@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">  
+
+    <xacro:include filename="$(find ati_force_torque)/description/urdf/fts.transmission.xacro" />
+
     <xacro:macro name="ati_mini58" params="parent name *origin">
 
 <!--         Shift for -30 -90 -->

description/urdf/sensor_mini58.urdf.xacro

@@ -1,14 +1,29 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
-       name="ati_mini58" >
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ati_mini58" >
 
   <!-- ati_mini58 -->
-  <xacro:include filename="$(find ati_force_torque)/description/urdf/macros/mini58.urdf.xacro" />
+
+  <xacro:if value="$(arg sim)">
+	  <xacro:include filename="$(find ati_force_torque)/description/urdf/macros/mini58_sim.urdf.xacro" />
+
+      <!-- Used for sensor simulation -->
+    <xacro:macro name="default_inertial">
+        <inertial>
+            <mass value="0.01"/>
+            <origin xyz="0 0 0"/>
+            <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
+        </inertial>
+    </xacro:macro>
+
+  </xacro:if>
+  <xacro:unless value="$(arg sim)">
+	  <xacro:include filename="$(find ati_force_torque)/description/urdf/macros/mini58.urdf.xacro" />
+  </xacro:unless>
 
   <!-- Used for fixing sensor to Gazebo -->
   <link name="base_link"/>
 
-  <!-- sensor -->
+<!-- sensor -->
   <xacro:ati_mini58 name="fts" parent="base_link">
       <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:ati_mini58>

package.xml

@@ -37,6 +37,8 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>visualization_msgs</depend>
+  <depend>realtime_tools</depend>
+  <depend>filters</depend>
 
   <exec_depend>message_runtime</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>

ros/include/ati_force_torque/force_torque_sensor.h

@@ -76,8 +76,10 @@ typedef unsigned char uint8_t;
 #include <iirob_filters/gravity_compensation.h>
 #include <iirob_filters/GravityCompensationParameters.h>
 #include <iirob_filters/low_pass_filter.h>
-#include <iirob_filters/moving_mean_filter.h>
 #include <iirob_filters/threshold_filter.h>
+#include <iirob_filters/moving_mean_filter.h>
+#include <iirob_filters/iirob_filter_base.h>
+#include <iirob_filters/iirob_filter_chain.h>
 
 #include <math.h>
 #include <iostream>
@@ -87,11 +89,15 @@ typedef unsigned char uint8_t;
 #include <ati_force_torque/CanConfigurationParameters.h>
 #include <ati_force_torque/FTSConfigurationParameters.h>
 #include <ati_force_torque/PublishConfigurationParameters.h>
-#include <ati_force_torque/PublishConfigurationConfig.h>
 #include <ati_force_torque/NodeConfigurationParameters.h>
 #include <ati_force_torque/CalibrationParameters.h>
 #include <ati_force_torque/CalibrationConfig.h>
 
+#include <filters/filter_chain.h>
+#include <filters/filter_base.h>
+#include <filters/mean.h>
+#include <realtime_tools/realtime_publisher.h>
+
 #define PI 3.14159265
 
 class ForceTorqueSensor
@@ -145,7 +151,8 @@ class ForceTorqueSensor
   geometry_msgs::Wrench offset_;
   geometry_msgs::TransformStamped transform_ee_base_stamped;
   tf2_ros::Buffer *p_tfBuffer;
-  ros::Publisher gravity_compensated_pub_, threshold_filtered_pub_, transformed_data_pub_, sensor_data_pub_, low_pass_pub_, moving_mean_pub_;
+  realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>  *gravity_compensated_pub_, *threshold_filtered_pub_, *transformed_data_pub_, *sensor_data_pub_, *low_pass_pub_, *moving_mean_pub_;
+
   bool is_pub_gravity_compensated_ = false;
   bool is_pub_threshold_filtered_ = false;
   bool is_pub_transformed_data_ = false;
@@ -160,7 +167,7 @@ class ForceTorqueSensor
   std::string canPath;
   int canBaudrate;
   int ftsBaseID;
-  double nodePubFreq;
+  double nodePubFreq, nodePullFreq;
   uint calibrationNMeasurements;
   double calibrationTBetween;
   int coordinateSystemNMeasurements;
@@ -188,42 +195,21 @@ class ForceTorqueSensor
   bool m_staticCalibration;
   geometry_msgs::Wrench m_calibOffset;
 
-  ThresholdFilter threshold_filter_;
-
-  LowPassFilter lp_filter_force_x_;
-  LowPassFilter lp_filter_force_y_;
-  LowPassFilter lp_filter_force_z_;
-  LowPassFilter lp_filter_torque_x_;
-  LowPassFilter lp_filter_torque_y_;
-  LowPassFilter lp_filter_torque_z_;
-  MovingMeanFilter moving_mean_filter_force_x_;
-  MovingMeanFilter moving_mean_filter_force_y_;
-  MovingMeanFilter moving_mean_filter_force_z_;
-  MovingMeanFilter moving_mean_filter_torque_x_;
-  MovingMeanFilter moving_mean_filter_torque_y_;
-  MovingMeanFilter moving_mean_filter_torque_z_;
-
-  GravityCompensator gravity_compensator_; 
-
-  bool useLowPassFilterForceX=false;  
-  bool useLowPassFilterForceY=false;
-  bool useLowPassFilterForceZ=false;
-  bool useLowPassFilterTorqueX=false;
-  bool useLowPassFilterTorqueY=false;
-  bool useLowPassFilterTorqueZ=false;
-  bool useMovinvingMeanForceX= false;
-  bool useMovinvingMeanForceY= false;
-  bool useMovinvingMeanForceZ= false;
-  bool useMovinvingMeanTorqueX= false;
-  bool useMovinvingMeanTorqueY= false;
-  bool useMovinvingMeanTorqueZ= false;
+
+  iirob_filters::IIrobFilterChain<geometry_msgs::WrenchStamped, iirob_filters::IIrobFilterBase<geometry_msgs::WrenchStamped>> low_pass_filter_;
+  iirob_filters::IIrobFilterChain<geometry_msgs::WrenchStamped, iirob_filters::IIrobFilterBase<geometry_msgs::WrenchStamped>> moving_mean_filter_;
+  iirob_filters::IIrobFilterChain<geometry_msgs::WrenchStamped, iirob_filters::IIrobFilterBase<geometry_msgs::WrenchStamped>> threshold_filter_;
+  iirob_filters::IIrobFilterChain<geometry_msgs::WrenchStamped, iirob_filters::IIrobFilterBase<geometry_msgs::WrenchStamped>> gravity_compensator_;  
+
   bool useGravityCompensator=false;
   bool useThresholdFilter=false;
+
+  bool useMovingMean = false;
+  bool useLowPassFilter = false;
+
 
-  dynamic_reconfigure::Server<ati_force_torque::CalibrationConfig> reconfigCalibrationSrv_; // Dynamic reconfiguration service
-  dynamic_reconfigure::Server<ati_force_torque::PublishConfigurationConfig> reconfigPublishSrv_; // Dynamic reconfiguration service
+  dynamic_reconfigure::Server<ati_force_torque::CalibrationConfig> reconfigCalibrationSrv_; // Dynamic reconfiguration service  
 
-  void reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level);
-  void reconfigurePublishRequest(ati_force_torque::PublishConfigurationConfig& config, uint32_t level);
+  void reconfigureCalibrationRequest(ati_force_torque::CalibrationConfig& config, uint32_t level);  
 };
 

ros/launch/driver.launch

@@ -4,7 +4,8 @@
     <arg name="sim" default="false"/>
 
     <!-- send urdf to param server -->
-    <param name="sensor_description" command="$(find xacro)/xacro --inorder '$(find ati_force_torque)/description/urdf/sensor.urdf.xacro'" unless = "$(arg sim)"/> 
+    <arg name="scenario_parameters" value="sim:=$(arg sim)"/> 
+    <param name="sensor_description" command="$(find xacro)/xacro --inorder '$(find ati_force_torque)/description/urdf/sensor_mini58.urdf.xacro' $(arg scenario_parameters)"/>  
 
     <!-- sensor state publisher -->
     <node ns="fts" pkg="robot_state_publisher" type="robot_state_publisher" name="sensor_state_publisher">