diff --git a/rbs_bt_executor/src/PoseEstimation.cpp b/rbs_bt_executor/src/PoseEstimation.cpp index 92eba20..fee2fe2 100644 --- a/rbs_bt_executor/src/PoseEstimation.cpp +++ b/rbs_bt_executor/src/PoseEstimation.cpp @@ -9,7 +9,6 @@ #include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" -// #include using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState; class PoseEstimation : public BtService { @@ -21,7 +20,17 @@ public: _set_params_client = this->_node->create_client( "/image_sub2/set_parameters"); - + + while (!_set_params_client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(_node->get_logger(), + "Interrupted while waiting for the service. Exiting."); + break; + } + RCLCPP_WARN(_node->get_logger(), + "service not available, waiting again..."); + } + _model_name = getInput("ObjectName").value(); } @@ -87,28 +96,24 @@ private: void set_mesh_param() { auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception"); - RCLCPP_INFO_STREAM(_node->get_logger(), _package_name); _model_path = build_model_path(_model_name, _package_name); + RCLCPP_INFO_STREAM(_node->get_logger(), _model_path); auto param_request = std::make_shared(); - rcl_interfaces::msg::ParameterValue val; - val.set__string_value(_model_path); + rcl_interfaces::msg::ParameterValue model_path_param_value; + model_path_param_value.set__string_value(_model_path); _param.name = "mesh_path"; - _param.value = val; + _param.value = model_path_param_value; _params.push_back(_param); param_request->set__parameters(_params); - while (!_set_params_client->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(_node->get_logger(), - "Interrupted while waiting for the service. Exiting."); - break; - } - RCLCPP_INFO(_node->get_logger(), - "service not available, waiting again..."); - } auto response = send_request(_node, _set_params_client, param_request); + RCLCPP_INFO(_node->get_logger(), "Response"); + + for (auto &tt : response->results) { + RCLCPP_INFO(_node->get_logger(), "%d \n", tt.successful); + } } rcl_interfaces::srv::SetParameters::Response::SharedPtr send_request( @@ -116,14 +121,14 @@ private: rclcpp::Client::SharedPtr client, rcl_interfaces::srv::SetParameters::Request::SharedPtr request) { auto result = client->async_send_request(request); - // Wait for the result. if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { return result.get(); } else { - return NULL; + return nullptr; } } + }; #include "behaviortree_cpp_v3/bt_factory.h"