Commit 4d159967 authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

new measurement script and filename decoding

use literal '{rospkg ilc}' for path to ilc
use literal '{time %F} for current date (cf. std::sftime)
parent 96ed8141
......@@ -17,6 +17,7 @@ find_package(Eigen REQUIRED)
......@@ -54,6 +54,23 @@
<type id="script_trigger_ilc_meas" prototype="script_trigger_ilc">
<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>
<type id="setpoint" prototype="list:float_parameter">
<member id="description">The constant setpoints for the respective measurement</member>
<type id="script_retime_ilc" prototype="script_service_caller">
#!/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,16 +24,80 @@ 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 decode_filename(self, filename):
name = filename
# Find timestamp and replace according to formating
for matchObj in re.finditer("{(\w*) ([^\}]*)}", name):
if == "time":
if != "":# found custom formating
# Replace timestamp according to input parameters
name = name.replace(, "{}".format(
# Replace timestamp according to predefined (general) parameters
name = name.replace(,
elif == "rospkg":
rospack = rospkg.RosPack()
name = name.replace(, "{}".format(rospack.get_path(
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))
def callback_ilc_calc_meas(self, req):
path = self.decode_filename(req.file_in)
### Read measurement data
data_org, header =
### Create error from measurement - setpoint
data_org[:, 1:] = data_org[:, 1:] - req.setpoint
### Continue as if nothing happend
message = self.ilc_calc(req, data_org, header)
return TriggerILCMeasResponse(success=True, message=message)
except Exception as e:
message = str(e)
return TriggerILCMeasResponse(success=False, message=message)
def callback_ilc_calc(self, req):
path = self.decode_filename(req.file_in)
### Read Error data
data_org, header =
data_org, header =
self.ilc_calc(req, data_org, header)
return TriggerILCResponse(success=True, message=message)
except Exception as e:
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)
......@@ -47,8 +113,9 @@ class Ilc_serviceproxy:
data_ff = ilc_lib.ilc_calculations(self.data_diff, req.L)
### Combine with last iteration, if existing
if self.initialized and os.path.exists(req.file_out):
last_ff, _ =
path_out = self.decode_filename(req.file_out)
if self.initialized and os.path.exists(path_out):
last_ff, _ =
# Reshape last feedforward if wanted
if self.reshape:
......@@ -65,7 +132,8 @@ class Ilc_serviceproxy:
data_ilc_q = ilc_lib.butter_lowpass_filter(data_ilc, req.cutoff_freq, self.fs, 5)
ilc_lib.write(req.file_out, data_ilc_q, header)
ilc_lib.write(path_out, data_ilc_q, header)
message = "file written sucessfully"
if self.folder is not None:
......@@ -76,11 +144,8 @@ class Ilc_serviceproxy:
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 TriggerILCResponse(success=True, message=message)
return message
except Exception as e:
message = str(e)
return TriggerILCResponse(success=False, message=message)
def callback_ilc_retime(self, req):
self.last_decay_time = self.decay_time
#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>
ScriptTriggerILCMeas(const string& name)
: ScriptRosService<ilc::TriggerILCMeas, ilc::TriggerILCMeas::Request, ilc::TriggerILCMeas::Response>(
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;
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>
......@@ -124,5 +174,6 @@ private:
int reuse_scheme_;
string file_in
float64[] setpoint
string file_out
string file_traj
string traj_type
float64 L
float64 cutoff_freq
bool success
string message
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment