Commit 1d935321 authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

FIX: ilc setpoint generator with rossrvs

parent b4706249
......@@ -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"/>
......
......@@ -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>
......
......@@ -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_;
};
......
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