Revision history [back]

Solved. Problem comes from configuration.

Original:

teb_local_planner::TebLocalPlannerReconfigureConfig new_config;
new_config.max_vel_x = new_vel_x;
new_config.max_vel_x_backwards = new_vel_x_backwards;
new_config.max_vel_theta = new_vel_theta;
dynamic_reconfigure_client_->setConfiguration(new_config);

Changed to:

teb_local_planner::TebLocalPlannerReconfigureConfig new_config;
ros::Duration wait_time(0.1);
dynamic_reconfigure_client_->getCurrentConfiguration(new_config, wait_time);
new_config.max_vel_x = new_vel_x;
new_config.max_vel_x_backwards = new_vel_x_backwards;
new_config.max_vel_theta = new_vel_theta;
dynamic_reconfigure_client_->setConfiguration(new_config);

Original code makes new_config include only max_vel parameters so that TEB can't retrieve other parameters.

Therefore, retrieve and store current configuration into new_config first and then modify the parameters which need to be updated.