robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

update pose estimation client

Browse source
This commit is contained in:
Ilya Uraev 2023-11-14 16:51:03 +03:00
parent 26269b6678
commit 578b503d32
1 changed files with 22 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

39
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -9,7 +9,6 @@
#include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/change_state.hpp"
// #include <component_interfaces/srv/detect_object.hpp>
using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState; using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
class PoseEstimation : public BtService<PoseEstimationSrv> { class PoseEstimation : public BtService<PoseEstimationSrv> {
@ -21,7 +20,17 @@ public:
_set_params_client = _set_params_client =
this->_node->create_client<rcl_interfaces::srv::SetParameters>( this->_node->create_client<rcl_interfaces::srv::SetParameters>(
"/image_sub2/set_parameters"); "/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<std::string>("ObjectName").value(); _model_name = getInput<std::string>("ObjectName").value();
} }
@ -87,28 +96,24 @@ private:
void set_mesh_param() { void set_mesh_param() {
auto _package_name = auto _package_name =
ament_index_cpp::get_package_share_directory("rbs_perception"); 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); _model_path = build_model_path(_model_name, _package_name);
RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
auto param_request = auto param_request =
std::make_shared<rcl_interfaces::srv::SetParameters::Request>(); std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
rcl_interfaces::msg::ParameterValue val; rcl_interfaces::msg::ParameterValue model_path_param_value;
val.set__string_value(_model_path); model_path_param_value.set__string_value(_model_path);
_param.name = "mesh_path"; _param.name = "mesh_path";
_param.value = val; _param.value = model_path_param_value;
_params.push_back(_param); _params.push_back(_param);
param_request->set__parameters(_params); 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); 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( rcl_interfaces::srv::SetParameters::Response::SharedPtr send_request(
@ -116,14 +121,14 @@ private:
rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client, rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client,
rcl_interfaces::srv::SetParameters::Request::SharedPtr request) { rcl_interfaces::srv::SetParameters::Request::SharedPtr request) {
auto result = client->async_send_request(request); auto result = client->async_send_request(request);
// Wait for the result.
if (rclcpp::spin_until_future_complete(node, result) == if (rclcpp::spin_until_future_complete(node, result) ==
rclcpp::FutureReturnCode::SUCCESS) { rclcpp::FutureReturnCode::SUCCESS) {
return result.get(); return result.get();
} else { } else {
return NULL; return nullptr;
} }
} }
}; };
#include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/bt_factory.h"