new measurement script and filename decoding

use literal '{rospkg ilc}' for path to ilc
use literal '{time %F} for current date (cf. std::sftime)

CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(Eigen REQUIRED)
 add_service_files(
   FILES
   TriggerILC.srv
+  TriggerILCMeas.srv
   RetimeILC.srv
   RetimeIfILC.srv
 )

models/ilc_scripts.xml

@@ -54,8 +54,25 @@
             </data>
         </type>
 
+        <type id="script_trigger_ilc_meas" prototype="script_trigger_ilc">
+            <meta>
+                <member id="description">Triggers a ros script to generate a file including ilc feedforward (file_out) based on given measeurement loggings (file_in) and the setpoint</member>
+                <member id="implementation">
+                    <clone prototype="orocos">
+                        <member id="package">ilc_scripts</member>
+                        <member id="component">ScriptTriggerILCMeas</member>
+                    </clone>
+                </member>
+            </meta>
+            <data>
+                <type id="setpoint" prototype="list:float_parameter">
+                    <member id="description">The constant setpoints for the respective measurement</member>
+                </type>
+            </data>
+        </type>
+
 
-    <type id="script_retime_ilc" prototype="script_service_caller">
+        <type id="script_retime_ilc" prototype="script_service_caller">
             <meta>
                 <member id="description">Commands a ilc_proxy to change the duration of the setpoint trajectory to the given length.</member>
                 <member id="implementation">
@@ -96,7 +113,7 @@
                 <type id="retime_factor" prototype="float_parameter">
                     <member id="description">Wanted duration reshaping to duration * [0..1]</member>
                 </type>
-                
+
                 <type id="convergence_threshold" prototype="float_parameter">
                     <member id="description">Apply reshaping if convergence is below this parameter</member>
                 </type>

scripts/ilc_lib/ilc_rosproxy.py

 #!/usr/bin/env python
 
 from __future__ import print_function
-from ilc.srv import TriggerILC, TriggerILCResponse, RetimeILC, RetimeILCResponse, RetimeIfILC, RetimeIfILCResponse
+from ilc.srv import *
 import rospy
 import numpy as np
 import ilc_lib
 import os
-import argparse
+import re
+import rospkg
+import datetime
 
 class Ilc_serviceproxy:
 
@@ -22,59 +24,71 @@ class Ilc_serviceproxy:
         self.fs = 1.0/(self.T * 0.001) # sampling frequency
 
         rospy.Service('ilc_service', TriggerILC, self.callback_ilc_calc)
+        rospy.Service('ilc_service_meas', TriggerILCMeas, self.callback_ilc_calc_meas)
         rospy.Service('ilc_retime', RetimeILC, self.callback_ilc_retime)
         rospy.Service('ilc_retime_if', RetimeIfILC, self.callback_ilc_retime_if)
 
         self.folder = rospy.get_param('~folder', None)
 
-    def callback_ilc_calc(self, req):
-        try:
-            ### Read Error data
-            data_org, header = ilc_lib.read(req.file_in)
 
-            ### Resample
-            data_rs = ilc_lib.resample(data_org, self.T)
+    def decode_filename(self, filename):
+        name = filename
+        # Find timestamp and replace according to formating
+        for matchObj in re.finditer("{(\w*) ([^\}]*)}", name):
+            if matchObj.group(1) == "time":
+                if matchObj.group(2) != "":# found custom formating
+                    # Replace timestamp according to input parameters
+                    name = name.replace(matchObj.group(), "{}".format(datetime.datetime.now().strftime(matchObj.group(2))))
+                else:
+                    # Replace timestamp according to predefined (general) parameters
+                    name = name.replace(matchObj.group(),
+                                        "{}".format(
+                                            datetime.datetime.now().strftime(
+                                                "%Y-%m-%d_%H:%M:%S")))
+            elif matchObj.group(1) == "rospkg":
+                rospack = rospkg.RosPack()
+                name = name.replace(matchObj.group(), "{}".format(rospack.get_path(matchObj.group(2))))
+
+        print("Decoded Name: {} -> {}".format(filename, name))
+        return name
+
+
+    def verify_file_path(self, name):
+        # Check if folder exists, otherwise create it
+        path = os.path.dirname(name)
+        if path and not os.path.isdir(path):
+            print("Creating new folder: {}".format(path))
+            os.makedirs(path)
+
+
+    def callback_ilc_calc_meas(self, req):
+        try:
+            path = self.decode_filename(req.file_in)
 
-            ### Trajectory generation
-            if self.decay_time is None:
-                self.decay_time = data_rs[-1, 0]
-            trajectory = ilc_lib.traj_handle(data_rs, "cos", decay_time=self.decay_time)
+            ### Read measurement data
+            data_org, header = ilc_lib.read(path)
 
-            ### Relative Error: data-trajectory
-            self.data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff")
+            ### Create error from measurement - setpoint
+            data_org[:, 1:] = data_org[:, 1:] - req.setpoint
 
-            ### ILC Gain
-            data_ff =  ilc_lib.ilc_calculations(self.data_diff, req.L)
+            ### Continue as if nothing happend
+            message = self.ilc_calc(req, data_org, header)
 
-            ### Combine with last iteration, if existing
-            if self.initialized and os.path.exists(req.file_out):
-                last_ff, _ = ilc_lib.read(req.file_out)
+            return TriggerILCMeasResponse(success=True, message=message)
 
-                # Reshape last feedforward if wanted
-                if self.reshape:
-                    last_ff = self.reshape_ff(last_ff, self.last_decay_time, self.decay_time)
-                    self.reshape = False
+        except Exception as e:
+            message = str(e)
+            return TriggerILCMeasResponse(success=False, message=message)
 
-                data_ff, last_ff = ilc_lib.pad_data(data_ff, last_ff)
-                data_ilc = ilc_lib.combine_data(data_ff, last_ff, "sum")
-            else: # initialized == False -> cannot combine with previous csv
-                data_ilc = data_ff
-                self.initialized = True
 
-            #Filter data
-            data_ilc_q = ilc_lib.butter_lowpass_filter(data_ilc, req.cutoff_freq, self.fs, 5)
+    def callback_ilc_calc(self, req):
+        try:
+            path = self.decode_filename(req.file_in)
 
-            #Write
-            ilc_lib.write(req.file_out, data_ilc_q, header)
-            message = "file written sucessfully"
+            ### Read Error data
+            data_org, header = ilc_lib.read(path)
 
-            if self.folder is not None:
-                ilc_lib.external_data_store(self.folder + "/read_error",        "read_error", data_org, header)
-                ilc_lib.external_data_store(self.folder + "/resampled_error",   "resampled_error", data_rs ,header)
-                ilc_lib.external_data_store(self.folder + "/trajectory",        "trajectory", trajectory, header)
-                ilc_lib.external_data_store(self.folder + "/ff_data",           "ff_data", data_ff, header)
-                ilc_lib.external_data_store(self.folder + "/ilc_data",          "ilc_data", data_ilc, header)
-                ilc_lib.external_data_store(self.folder + "/filtered_ilc_data", "filtered_ilc_data", data_ilc_q, header)
+            self.ilc_calc(req, data_org, header)
 
             return TriggerILCResponse(success=True, message=message)
 
@@ -82,6 +96,57 @@ class Ilc_serviceproxy:
             message = str(e)
             return TriggerILCResponse(success=False, message=message)
 
+
+    def ilc_calc(self, req, data_org, header):
+        ### Resample
+        data_rs = ilc_lib.resample(data_org, self.T)
+
+        ### Trajectory generation
+        if self.decay_time is None:
+            self.decay_time = data_rs[-1, 0]
+        trajectory = ilc_lib.traj_handle(data_rs, "cos", decay_time=self.decay_time)
+
+        ### Relative Error: data-trajectory
+        self.data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff")
+
+        ### ILC Gain
+        data_ff =  ilc_lib.ilc_calculations(self.data_diff, req.L)
+
+        ### Combine with last iteration, if existing
+        path_out = self.decode_filename(req.file_out)
+        if self.initialized and os.path.exists(path_out):
+            last_ff, _ = ilc_lib.read(path_out)
+
+            # Reshape last feedforward if wanted
+            if self.reshape:
+                last_ff = self.reshape_ff(last_ff, self.last_decay_time, self.decay_time)
+                self.reshape = False
+
+            data_ff, last_ff = ilc_lib.pad_data(data_ff, last_ff)
+            data_ilc = ilc_lib.combine_data(data_ff, last_ff, "sum")
+        else: # initialized == False -> cannot combine with previous csv
+            data_ilc = data_ff
+            self.initialized = True
+
+        #Filter data
+        data_ilc_q = ilc_lib.butter_lowpass_filter(data_ilc, req.cutoff_freq, self.fs, 5)
+
+        #Write
+        self.verify_file_path(path_out)
+        ilc_lib.write(path_out, data_ilc_q, header)
+        message = "file written sucessfully"
+
+        if self.folder is not None:
+            ilc_lib.external_data_store(self.folder + "/read_error",        "read_error", data_org, header)
+            ilc_lib.external_data_store(self.folder + "/resampled_error",   "resampled_error", data_rs ,header)
+            ilc_lib.external_data_store(self.folder + "/trajectory",        "trajectory", trajectory, header)
+            ilc_lib.external_data_store(self.folder + "/ff_data",           "ff_data", data_ff, header)
+            ilc_lib.external_data_store(self.folder + "/ilc_data",          "ilc_data", data_ilc, header)
+            ilc_lib.external_data_store(self.folder + "/filtered_ilc_data", "filtered_ilc_data", data_ilc_q, header)
+
+        return message
+
+
     def callback_ilc_retime(self, req):
         self.last_decay_time = self.decay_time
         self.decay_time = req.setpoint_duration_ms

src/ilc/scripts_ilc.cpp

 #include "cppitasc/coordination/script_rosservice.hpp"
 
 #include <ilc/TriggerILC.h>
+#include <ilc/TriggerILCMeas.h>
 #include <ilc/RetimeILC.h>
 #include <ilc/RetimeIfILC.h>
 
@@ -50,6 +51,55 @@ private:
 };
 
 
+class ScriptTriggerILCMeas : public ScriptRosService<ilc::TriggerILCMeas, ilc::TriggerILCMeas::Request, ilc::TriggerILCMeas::Response>
+{
+public:
+    ScriptTriggerILCMeas(const string& name)
+        : ScriptRosService<ilc::TriggerILCMeas, ilc::TriggerILCMeas::Request, ilc::TriggerILCMeas::Response>(
+            name)
+    {
+    }
+
+    bool init(Dict& params) override {
+        extract(params["file_in"], fin_);
+        extract(params["setpoint"], setpoint_);
+        extract(params["file_out"], fout_);
+        extract(params["file_traj"], trajout_);
+        extract(params["traj_type"], trajtype_);
+        extract(params["L"], Lgain_);
+        extract(params["cutoff_freq"], cutoff_);
+
+        service_.request.file_in = fin_;
+        service_.request.setpoint = setpoint_;
+        service_.request.file_out = fout_;
+        service_.request.file_traj = trajout_;
+        service_.request.traj_type = trajtype_;
+        service_.request.L = Lgain_;
+        service_.request.cutoff_freq = cutoff_;
+
+
+        return ScriptRosService::init(params);
+    }
+
+    bool call() override {
+        bool result = ScriptRosService<ilc::TriggerILCMeas, ilc::TriggerILCMeas::Request, ilc::TriggerILCMeas::Response>::call();
+        if (!result) {
+            pi_error("Message: {}", service_.response.message);
+        }
+        return service_.response.success;
+    }
+
+private:
+    string fin_;
+    vector<double> setpoint_;
+    string fout_;
+    string trajout_;
+    string trajtype_;
+    double Lgain_;
+    double cutoff_;
+};
+
+
 class ScriptRetimeILC : public ScriptRosService<ilc::RetimeILC, ilc::RetimeILC::Request, ilc::RetimeILC::Response>
 {
 public:
@@ -81,7 +131,7 @@ public:
 
 private:
     int setpoint_duration_ms_;
-    bool reuse_ff_; 
+    bool reuse_ff_;
     int reuse_scheme_;
 };
 
@@ -120,9 +170,10 @@ public:
 private:
     double retime_factor_;
     double convergence_threshold_;
-    bool reuse_ff_; 
+    bool reuse_ff_;
     int reuse_scheme_;
 };
 RUNTIME_COMPONENT(ScriptTriggerILC)
+RUNTIME_COMPONENT(ScriptTriggerILCMeas)
 RUNTIME_COMPONENT(ScriptRetimeILC)
 RUNTIME_COMPONENT(ScriptRetimeIfILC)

srv/TriggerILCMeas.srv

+string file_in
+float64[] setpoint
+string file_out
+string file_traj
+string traj_type
+float64 L
+float64 cutoff_freq
+----------------------
+bool success
+string message