Dynamic reconfigure in local_planner

1

Hi,

I’m trying to change some of the px4 avoidance local_planner parameters via dynamic reconfigure during the flight of the IRIS in SITL. Let’s say I want to change the goal_z_param from 3 (default) to 6.
Flying the IRIS from start to goal without any changes in the code works fine.
Changing goal_z_param from the terminal with rosrun dynamic_reconfigure dynparam set local_planner_nodelet goal_z_param 6 mid flight also works fine.
Then I wrote a small tool, which sets goal_z_param to 6 when called from the terminal (e.g. ‘rosrun local_planner set_z’).

#include <local_planner/set_z.h>

namespace avoidance {
void set_z_fun(std::string para_name, int para_value) {
  dynamic_reconfigure::ReconfigureRequest srv_req;
  dynamic_reconfigure::ReconfigureResponse srv_resp;
  dynamic_reconfigure::DoubleParameter param;
  dynamic_reconfigure::Config conf;

  param.name = para_name;
  param.value = para_value;
  conf.doubles.push_back(param);

  srv_req.config = conf;

  if (ros::service::call("/local_planner_nodelet/set_parameters", srv_req, srv_resp)) {
    ROS_INFO("call to set local_planner parameters succeeded");
  } else {
    ROS_INFO("call to set local_planner parameters failed");
  }
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "set_z");
  set_z_fun("goal_z_param", 6);
  return 0;
} 
}

Calling this function from the terminal also works fine.

If I try to call set_z_fun in the local_planner_nodelet.cpp the local_planner_manager-11 dies. I also copied the core-code of set_z_fun into the local_planner_nodelet. However, it doesn’t have any effect (e.g. goal_z_param remains 3) but any ros command from the terminal times out.
I also tried to directly use the LocalPlanner::dynmicReconfigureSetParams function. My code compiles but doesn’t have any effect (e.g. goal_z_param remains 3). And here again all terminal ros commands time out.

Does anybody have an idea where my problem might be?

Thanks a lot
Andy