From 45dc36294d58d0be68b02ba81b600023a4fa268e Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 24 Nov 2022 22:49:40 +0900 Subject: [PATCH] check if dyn param server is up before setting --- detect_cans_in_fridge_201202/euslisp/main.l | 22 +++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/detect_cans_in_fridge_201202/euslisp/main.l b/detect_cans_in_fridge_201202/euslisp/main.l index b12d3d64ae..dc10b34814 100644 --- a/detect_cans_in_fridge_201202/euslisp/main.l +++ b/detect_cans_in_fridge_201202/euslisp/main.l @@ -37,14 +37,20 @@ (defun restore-params () - (if (boundp '*global-inflation-radius*) - (ros::set-dynamic-reconfigure-param - "/move_base_node/global_costmap/inflation_layer" "inflation_radius" - :double *global-inflation-radius*)) - (if (boundp '*local-inflation-radius*) - (ros::set-dynamic-reconfigure-param - "/move_base_node/local_costmap/inflation_layer" "inflation_radius" - :double *local-inflation-radius*)) + (let ((global-costmap-server "/move_base_node/global_costmap/inflation_layer") + (local-costmap-server "/move_base_node/local_costmap/inflation_layer")) + (if (and (boundp '*global-inflation-radius*) + *global-inflation-radius* + (ros::wait-for-service (format nil "~A/set_parameters" global-costmap-server) 1)) + (ros::set-dynamic-reconfigure-param + global-costmap-server "inflation_radius" + :double *global-inflation-radius*)) + (if (and (boundp '*local-inflation-radius*) + *local-inflation-radius* + (ros::wait-for-service (format nil "~A/set_parameters" local-costmap-server) 1)) + (ros::set-dynamic-reconfigure-param + local-costmap-server "inflation_radius" + :double *local-inflation-radius*))) t)