Added support for RS-485 communication (#11)

* Added support for RS-485 communication

Adjusted the sensor handle to dynamically use CAN or RS-485 communication, depending on the plugin loaded.
Also added a Configuration file for the RS485 interface.

* Updated handles when testing RS485 communication

* generated changelog

* 0.0.1

* Publishing before merge

Author: FlorianAumann

CHANGELOG.rst

@@ -17,6 +17,8 @@ Changelog for package force_torque_sensor
   * Update .travis.rosinstall
 * Added Melodic in overview
 * Added travis config for melodic
+0.0.1 (2018-12-12)
+------------------
 * Update CMakeLists.txt
 * Updated INSTALL paths
 * Update CMakeLists.txt

CMakeLists.txt

@@ -42,7 +42,7 @@ endif()
 
 generate_ros_parameter_files(
     cfg/CoordinateSystemCalibration.params
-    cfg/CanConfiguration.params
+    cfg/HWCommunicationConfiguration.params
     cfg/FTSConfiguration.params
     cfg/NodeConfiguration.params
     cfg/PublishConfiguration.params

cfg/CanConfiguration.params

@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+from rosparam_handler.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("type", paramtype="int", description="communication type; important for CAN", default=-1, configurable=False)
+gen.add("path", paramtype="std::string", description="path", default=" ", configurable=False)
+gen.add("baudrate", paramtype="int", description="baudrate", default=-1, configurable=False)
+
+exit(gen.generate("force_torque_sensor", "ForceTorqueSensor", "HWCommunicationConfiguration"))

cfg/HWCommunicationConfiguration.params

@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+from rosparam_handler.parameter_generator_catkin import *
+gen = ParameterGenerator()
+
+gen.add("path", paramtype="std::string", description="path_rs", default=" ", configurable=False)
+gen.add("baudrate", paramtype="int", description="baudrate", default=-1, configurable=False)
+
+exit(gen.generate("force_torque_sensor", "ForceTorqueSensor", "RS485Configuration"))

cfg/RS485Configuration.params

@@ -0,0 +1,41 @@
+Node:
+    sim: false
+    sensor_hw: "force_torque_sensor/ForceTorqueSensorSim"
+    ft_pub_freq: 200
+    ft_pull_freq: 800
+    sensor_frame: "fts_reference_link"
+    transform_frame: "fts_base_link"
+
+Calibration:
+    Offset:
+        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:
+  name: ThresholdFilter
+  type: iirob_filters/ThresholdFilterWrench
+  params: {linear_threshold: 2.5,  angular_threshold: 0.3}
+
+LowPassFilter:
+  name: LowPassFilter
+  type: iirob_filters/LowPassFilterWrench
+  params: {SamplingFrequency: 200.0, DampingFrequency: 15.0, DampingIntensity: -6.0}
+        
+MovingMeanFilter:
+  name: MovingMeanFilter
+  type: filters/MovingMeanFilterWrench
+  params: {divider: 4}
+
+GravityCompensation:
+  name: GravityCompensationWrench
+  type: iirob_filters/GravityCompensator
+  params: {world_frame: "base_link"}
+
+Publish:
+    sensor_data: true
+    low_pass: true
+    moving_mean: true
+    transformed_data: true
+    gravity_compensated: true
+    threshold_filtered: true

config/sensor_configuration.yaml

@@ -1,4 +1,4 @@
-CAN:
+HWComm:
     type: 5           # CANITFTYPE_SOCKET_CAN
     path: can0
 

config/socket_can.yaml

@@ -79,7 +79,7 @@ typedef unsigned char uint8_t;
 
 #include <dynamic_reconfigure/server.h>
 #include <force_torque_sensor/CoordinateSystemCalibrationParameters.h>
-#include <force_torque_sensor/CanConfigurationParameters.h>
+#include <force_torque_sensor/HWCommunicationConfigurationParameters.h>
 #include <force_torque_sensor/FTSConfigurationParameters.h>
 #include <force_torque_sensor/PublishConfigurationParameters.h>
 #include <force_torque_sensor/NodeConfigurationParameters.h>
@@ -127,7 +127,7 @@ class ForceTorqueSensorHandle : public hardware_interface::ForceTorqueSensorHand
   double interface_torque_[3];
 
   force_torque_sensor::CoordinateSystemCalibrationParameters CS_params_;
-  force_torque_sensor::CanConfigurationParameters can_params_;
+  force_torque_sensor::HWCommunicationConfigurationParameters HWComm_params_;
   force_torque_sensor::FTSConfigurationParameters FTS_params_;
   force_torque_sensor::PublishConfigurationParameters pub_params_;
   force_torque_sensor::NodeConfigurationParameters node_params_;
@@ -166,10 +166,10 @@ class ForceTorqueSensorHandle : public hardware_interface::ForceTorqueSensorHand
 
   uint _num_transform_errors;
 
-  // CAN parameters
-  int canType;
-  std::string canPath;
-  int canBaudrate;
+  // HWComm parameters
+  int HWCommType; // Only important if can is used
+  std::string HWCommPath;
+  int HWCommBaudrate;
   int ftsBaseID;
   double nodePubFreq, nodePullFreq;
   uint calibrationNMeasurements;

include/force_torque_sensor/force_torque_sensor_handle.h

@@ -48,14 +48,14 @@
 using namespace force_torque_sensor;
 
 ForceTorqueSensorHandle::ForceTorqueSensorHandle(ros::NodeHandle& nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame) :
-    hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_), nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"}
+    hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_), nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, HWComm_params_{nh.getNamespace()+"/HWComm"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"}
 {
     p_Ftc = sensor;
     prepareNode(output_frame);
 }
 
 ForceTorqueSensorHandle::ForceTorqueSensorHandle(ros::NodeHandle& nh, std::string sensor_name, std::string output_frame) :
-    hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_), nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, can_params_{nh.getNamespace()+"/CAN"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"}
+    hardware_interface::ForceTorqueSensorHandle(sensor_name, output_frame, interface_force_, interface_torque_), nh_(nh), calibration_params_{nh.getNamespace()+"/Calibration/Offset"}, CS_params_{nh.getNamespace()}, HWComm_params_{nh.getNamespace()+"/HWComm"}, FTS_params_{nh.getNamespace()+"/FTS"}, pub_params_{nh.getNamespace()+"/Publish"}, node_params_{nh.getNamespace()+"/Node"}, gravity_params_{nh.getNamespace()+"/GravityCompensation/params"}
 {
     node_params_.fromParamServer();
 
@@ -94,7 +94,7 @@ void ForceTorqueSensorHandle::prepareNode(std::string output_frame)
 
     calibration_params_.fromParamServer();
     CS_params_.fromParamServer();
-    can_params_.fromParamServer();
+    HWComm_params_.fromParamServer();
     FTS_params_.fromParamServer();
     pub_params_.fromParamServer();
     node_params_.fromParamServer();
@@ -137,9 +137,9 @@ void ForceTorqueSensorHandle::prepareNode(std::string output_frame)
     srvServer_SetSensorOffset = nh_.advertiseService("SetSensorOffset", &ForceTorqueSensorHandle::srvCallback_setSensorOffset, this);
 
     // Read data from parameter server
-    canType = can_params_.type;
-    canPath = can_params_.path;
-    canBaudrate = can_params_.baudrate;
+    HWCommType = HWComm_params_.type;
+    HWCommPath = HWComm_params_.path;
+    HWCommBaudrate = HWComm_params_.baudrate;
     ftsBaseID = FTS_params_.base_identifier;
     isAutoInit = FTS_params_.auto_init;
     nodePubFreq = node_params_.ft_pub_freq;
@@ -205,8 +205,8 @@ void ForceTorqueSensorHandle::prepareNode(std::string output_frame)
         useThresholdFilter = true;
         threshold_filter_->configure(nh_.getNamespace()+"/ThresholdFilter");
     }
-
-    p_Ftc->initCommunication(canType, canPath, canBaudrate, ftsBaseID);
+    
+    p_Ftc->initCommunication(HWCommType, HWCommPath, HWCommBaudrate, ftsBaseID);
 
     if (isAutoInit)
     {
@@ -493,11 +493,11 @@ bool ForceTorqueSensorHandle::srvReadDiagnosticVoltages(force_torque_sensor::Dia
 
 void ForceTorqueSensorHandle::pullFTData(const ros::TimerEvent &event)
 {
-    int status = 0;
-
-    bool bRet = p_Ftc->readFTData(status, sensor_data.wrench.force.x, sensor_data.wrench.force.y, sensor_data.wrench.force.z,
-                                                        sensor_data.wrench.torque.x, sensor_data.wrench.torque.y, sensor_data.wrench.torque.z);
-    if (bRet != false)
+    ros::Time timestamp = ros::Time::now();
+    
+    if (p_Ftc->readFTData(0, sensor_data.wrench.force.x, sensor_data.wrench.force.y, sensor_data.wrench.force.z,
+                                                        sensor_data.wrench.torque.x, sensor_data.wrench.torque.y, sensor_data.wrench.torque.z)
+        != false)
     {
         sensor_data.header.stamp = ros::Time::now();
         sensor_data.header.frame_id = sensor_frame_;
@@ -513,19 +513,13 @@ void ForceTorqueSensorHandle::pullFTData(const ros::TimerEvent &event)
         //lowpass
         low_pass_filtered_data.header = sensor_data.header;
         if(useLowPassFilter){
-            std::vector<double> in_data= {(double)sensor_data.wrench.force.x, double(sensor_data.wrench.force.y), (double)sensor_data.wrench.force.z,(double)sensor_data.wrench.torque.x,(double)sensor_data.wrench.torque.y,(double)sensor_data.wrench.torque.z};
-            std::vector<double> out_data= {(double)low_pass_filtered_data.wrench.force.x, double(low_pass_filtered_data.wrench.force.y), (double)low_pass_filtered_data.wrench.force.z,(double)low_pass_filtered_data.wrench.torque.x,(double)low_pass_filtered_data.wrench.torque.y,(double)low_pass_filtered_data.wrench.torque.z};
-
             low_pass_filter_->update(sensor_data,low_pass_filtered_data);
         }
         else low_pass_filtered_data = sensor_data;
 
         //moving_mean
         moving_mean_filtered_wrench.header = low_pass_filtered_data.header;
         if(useMovingMean){
-            std::vector<double> in_data= {(double)low_pass_filtered_data.wrench.force.x, double(low_pass_filtered_data.wrench.force.y), (double)low_pass_filtered_data.wrench.force.z,(double)low_pass_filtered_data.wrench.torque.x,(double)low_pass_filtered_data.wrench.torque.y,(double)low_pass_filtered_data.wrench.torque.z};
-            std::vector<double> out_data = {(double)moving_mean_filtered_wrench.wrench.force.x, double(moving_mean_filtered_wrench.wrench.force.y), (double)moving_mean_filtered_wrench.wrench.force.z,(double)moving_mean_filtered_wrench.wrench.torque.x,(double)moving_mean_filtered_wrench.wrench.torque.y,(double)moving_mean_filtered_wrench.wrench.torque.z};
-
             moving_mean_filter_->update(low_pass_filtered_data, moving_mean_filtered_wrench);
         }
         else moving_mean_filtered_wrench = low_pass_filtered_data;
@@ -548,6 +542,8 @@ void ForceTorqueSensorHandle::pullFTData(const ros::TimerEvent &event)
                 moving_mean_pub_->unlockAndPublish();
             }
     }
+    
+//      std::cout << (ros::Time::now() - timestamp).toNSec()/1000.0 << " ms" << std::endl;
 }
 
 void ForceTorqueSensorHandle::filterFTData(){