Open Timple opened 4 years ago
Is 1/controller_frequency
not good enough?
I suggest let sampling time(dt) be a computeVelocityCmd() function argument. Because local planner role is like mobile robot motion controller also call rough velocity command interpolater, so I think sampling time is important. But in my experience, if sampling time is not the same will face strange behavior.
In addtion, I notice AbstractControllerExecution's callingduration is not the DEFAULT_CONTROLLER_FREQUENCY. when I running on my design local planner, it show the moving freqency is 0.05 but I didn't change DEFAULT_CONTROLLER_FREQUENCY. As shown below fiugre link. https://imgur.com/ZaLOiLa/
@Rayman I don't think we have access to the controller_frequency
@q576333 I disagree that dt
should be constant. Of course it should be kept as constant as possible by mbf. But I rather receive the real value than the setting.
Imagine a PID controller and for some reason a loop takes twice as long, now we've covered twice the distance, probably increased twice the error we would have done otherwise. If I now have a dt
which is also twice the value I still get a proportional output. When dt
is not changing I'm only doing have the effort.
If we go full-on we can do it like the ROS timers: pass the expected and real value.
The DWA planner does the following in dwa_planner.cpp. It might be useful:
//Assuming this planner is being run within the navigation stack, we can
//just do an upward search for the frequency at which its being run. This
//also allows the frequency to be overwritten locally.
std::string controller_frequency_param_name;
if(!private_nh.searchParam("controller_frequency", controller_frequency_param_name)) {
sim_period_ = 0.05;
} else {
double controller_frequency = 0;
private_nh.param(controller_frequency_param_name, controller_frequency, 20.0);
if(controller_frequency > 0) {
sim_period_ = 1.0 / controller_frequency;
} else {
ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
sim_period_ = 0.05;
}
}
Thanks! But trying to do our own parameter search seems hacky. And this which will break here I guess?
Ah, good point. It seems that in the original navigation stack controller_frequency
is not reconfigurable, so searchParam
works, but in MBF it is dynamic reconfigurable and so there could be an inconsistency, right?
I think cached parameters do the trick for you:
But I never tried them myself; and I'm not 100% they work with dynamically reconfigured parameters. Please report here if you try; I'm curious :neckbeard:
Even if it works, I still find it hacky to perform a search for a parameter name that might exist and then subscribe to the roscore to get updates from said parameter. While internally in the node the frequency is known and can just be passed along.
Well, you know current frequency, but as u said, is a dynamic param, so much better (and more precise) to get notified when it changes that continuously monitor the frequency by measuring the lag between calls. And is not really a search; you know the param is in level up relative to your plugin parameters namespace
@Timple Please open a PR and add an if-statement around https://github.com/magazino/move_base_flex/blob/9f9fd9c1b14ce56822118c820740ed22184b594a/mbf_abstract_nav/src/abstract_controller_execution.cpp#L399
with checking a parameter, which is set to true
in the default case to keep the current behaviour as default. Follow the naming of the other parameters. Any objections?
with checking a parameter, which is set to
true
in the default case to keep the current behaviour as default. Follow the naming of the other parameters. Any objections?
For @Timple's particular usecase I still find nicer to use cached parameters, but I'm ok with the param solution; we already have force_stop_at_goal
and force_stop_on_cancel
, so... force_stop_on_error
won't hurt.
@corot Room for thought, I like the actual dt
better than the set one. But doing nothing on the first run to achieve this feels hacky as well.
Nevertheless I'll implement the force_stop_on_error
parameter. Because a sudden 0.0 even on an error simply hurts our system (since I don't like smoothers catching these either).
We could also extend the plugin interface and provide the time delta.
I guess that would break backwards compatibility with all mbf local planner plugins? I'm not familiar enough yet with c++ if this can be done with overloading the virtual functions somehow.
For our local planner we need
dt
. So on the first call tocomputeVelocityCmd
we return false and save the timestamp. So every next loop can calculatedt
.But this causes a zero cmd_vel to be published. However often we are not standing still. So the zero cmd_vel is undesired.
Of course internally we can remember the last timestamp, but there is also no guarantee that this is not from a while back (sometimes we do stand still and do nothing).
Is there a proper way to handle this scenario?
if dt > 0.1: we probably stood still inbetween
dt
as argument tocomputeVelocityCmd