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

set 'mesh_path' from BT node

Browse source
This commit is contained in:
shalenikol 2023-11-15 21:07:32 +03:00
parent 578b503d32
commit c5f37c587f
2 changed files with 10 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

41
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -2,6 +2,7 @@
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/logging.hpp>
#include <string>
#include "rcl_interfaces/msg/parameter.hpp"
@ -17,9 +18,7 @@ public:
: BtService<PoseEstimationSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
_set_params_client =
this->_node->create_client<rcl_interfaces::srv::SetParameters>(
"/image_sub2/set_parameters");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "/image_sub2");
while (!_set_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
@ -62,8 +61,7 @@ private:
std::string _model_name{};
std::string _model_path{};
std::string _req_type{};
rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr
_set_params_client;
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::vector<rcl_interfaces::msg::Parameter> _params;
rcl_interfaces::msg::Parameter _param;
@ -94,39 +92,12 @@ private:
}
void set_mesh_param() {
auto _package_name =
ament_index_cpp::get_package_share_directory("rbs_perception");
auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
_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::srv::SetParameters::Request>();
rcl_interfaces::msg::ParameterValue model_path_param_value;
model_path_param_value.set__string_value(_model_path);
_param.name = "mesh_path";
_param.value = model_path_param_value;
_params.push_back(_param);
param_request->set__parameters(_params);
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(
rclcpp::Node::SharedPtr node,
rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client,
rcl_interfaces::srv::SetParameters::Request::SharedPtr request) {
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(node, result) ==
rclcpp::FutureReturnCode::SUCCESS) {
return result.get();
} else {
return nullptr;
}
std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("mesh_path", _model_path)};
_set_params_client->set_parameters(_parameters);
}
};

5
rbs_perception/scripts/pose_estimation_lifecycle.py
View file

@ -82,8 +82,11 @@ class PoseEstimator(Node):
self.mytemppath = Path(self.tmpdir) / "rbs_per"
self.mytemppath.mkdir(exist_ok=True)
# for other nodes
kwargs["allow_undeclared_parameters"] = True
kwargs["automatically_declare_parameters_from_overrides"] = True
super().__init__(self.nodeName, **kwargs)
self.declare_parameter("mesh_path", "")
self.declare_parameter("mesh_path", rclpy.Parameter.Type.STRING)
# Initialize the transform broadcaster
self.tf_broadcaster = TransformBroadcaster(self)