FIX: ilc setpoint generator with rossrvs

apps/rob_ilc_lin_test.xml

@@ -91,6 +91,7 @@
                                 <member id="coordinates" reference_id="coordinates"/>
                                 <member id="prefix" reference_id="prefix"/>
                                 <member id="duration" reference_id="trajectory_duration"/>
+                                <member id="retime_factor">0.9</member>
                                 <member id="max_vel_groups">
                                     <reference reference_id="max_linear_velocity"/>
                                     <reference reference_id="max_angular_velocity"/>

models/ilc_setpoint_gen.xml

@@ -20,6 +20,12 @@
                     </meta>
                     <data>0</data>
                 </type>
+                <type id="retime_factor" prototype="float_parameter">
+                    <meta>
+                        <member id="description">Factor to be applied on the setpoint duration if ros service is received.</member>
+                    </meta>
+                    <data>0.9</data>
+                </type>
                 <type id="symbol_groups" data_type="list:string_csv" prototype="base">
                     <meta>
                         <member id="description">List of symbol lists 'symbol_group' that belong to a common movement (e.g. [ [x,y,z],[a,b,c] ]).</member>

src/ilc/raising_cosine_setpoint_generator_ilc.cpp

@@ -18,16 +18,19 @@ public:
         : ConstSetpointGenerator(name),
         started_(false),
         duration_(0),
-        max_duration_(0)
+        max_duration_(0),
+        retime_factor_(1.0) // No retiming if not set per parameter
     {
+        name_ = name.substr(name.find_last_of("/")+1, name.size());
     }
 
     bool init(Dict& params) override {
         pi_debug_named("RaisingCosineSetpointGenerator::init", "component: '{}'", ConstSetpointGenerator::getName());
 
-        extract(params["duration"], duration_);
+        extract(params["duration"], duration_); // duration according to parameters
         extract(params["max_vel_groups"], max_vel_groups_);
         extract(params["symbol_groups"], symbol_groups_);
+        extract(params["retime_factor"], retime_factor_);
 
         return ConstSetpointGenerator::init(params);
 }
@@ -37,23 +40,22 @@ protected:
     bool onStart() override {
         started_ = false;
         distances_start_.assign(desiredCoordinates_.size(), 0);
-        max_duration_ = 0.0;
         return ConstSetpointGenerator::onStart();
     }
 
-    bool callback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
+    bool callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
     {
-        pi_info("Service with name {} called", serviceName_);
         called_ = true;
         started_ = false;
-        duration_ = 0.5 * duration_;
-        return true;
+        duration_ = retime_factor_ * duration_;
+        pi_info("Service recieved to retime trajectory to {}s", duration_);
+        return called_;
     }
 
     bool onConfigure() override {
-        pi_info("Service with name {} created", "serviceName_");
+        serviceName_ = "retime_" + name_;
+        pi_info("Service with name {} created", serviceName_);
         called_ = false;
-        serviceName_ = "blub_" + std::to_string(int(duration_));;
         service_ = n_.advertiseService(serviceName_, &RaisingCosineSetpointGeneratorILC::callback, this);
         return ConstSetpointGenerator::onConfigure();
     }
@@ -68,15 +70,14 @@ protected:
     void onUpdate(const Tick& tick) final {
         if (called_) {
             called_ = false;
-            pi_warn("CALLED:  {}", duration_);
-            pi_error("CALLED:  {}", duration_);
-            pi_warn("CALLED:  {}", duration_);
+            pi_warn("Setpoint trajectory retimed to {}s", duration_);
         }
 
         // Scaling on actual first measured value as being the current starting point
         // begin initialization
         if (!started_) {
-            unsigned int critical_idx;
+            max_duration_ = 0.0; // most critical duration of parameter (duration_) and velocity limits. Must be set to zero at init
+            unsigned int critical_idx = 0;
             string critical_coordinate;
             auto measuredValues = dataSource_->getData();
             // desiredValues_ is constant and defined in base class
@@ -104,14 +105,14 @@ protected:
                  * d = pi*s/(2*v_max) = pi/2 * s / v_max
                  */
                 auto duration = ( (M_PI_2 * std::abs(distance)) / max_vel_groups_[idx] );
+                critical_idx = idx;
+                critical_coordinate = c;
 
                 /* If any duration exceeds the parametrized duration, the velocity exceeds its limit
                  * The longest duration (with given velocity limitation) defines the length of the overall setpoint trajectory
                  */
                 if (duration > max_duration_) {
                     max_duration_ = duration;
-                    critical_idx = idx;
-                    critical_coordinate = c;
                 }
             }
             start_ = tick;
@@ -207,10 +208,12 @@ private:
     ros::ServiceServer service_;
 
     bool called_;
+    double retime_factor_;
 
 
 protected:
     string serviceName_;
+    string name_;
 
 };