mbf publishes zero cmd_vel if computeVelocityCmd returns False

[Timple]

For our local planner we need dt. So on the first call to computeVelocityCmd we return false and save the timestamp. So every next loop can calculate dt.

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?

• I can put in a PR which removes the zero publish
• I can remember the previous time and do ugly logic if dt > 0.1: we probably stood still inbetween
• Publish the current cmd_vel value again instead of zero (needs Compute current velocity for the controller. #34)
• Provide dt as argument to computeVelocityCmd
• Have some kind of flag if we are a direct follow-up from a previous path

[Rayman]

Is 1/controller_frequency not good enough?

[q576333]

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 calling_duration_ 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/

[Timple]

@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.

[Rayman]

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;
  }
}

[Timple]

Thanks! But trying to do our own parameter search seems hacky. And this which will break here I guess?

[Rayman]

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?

[corot]

I think cached parameters do the trick for you:

• http://wiki.ros.org/roscpp/Overview/Parameter%20Server#Cached%20Parameters
• https://answers.ros.org/question/9867/notification-when-a-parameter-in-param-server-is-changed/

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

[Timple]

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.

[corot]

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

[spuetz]

@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?

[corot]

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.

[Timple]

@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).

[spuetz]

We could also extend the plugin interface and provide the time delta.

[Timple]

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.