Merge branch 'main' into 77-cartesian-controllers

This commit is contained in:
Ilya Uraev 2024-02-16 15:10:28 +03:00
commit 9c56582d9f
12 changed files with 582 additions and 7 deletions

View file

@ -71,6 +71,9 @@ list(APPEND plugin_libs rbs_assemble_process_state)
add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
list(APPEND plugin_libs rbs_pose_estimation)
add_library(rbs_object_detection SHARED src/ObjectDetection.cpp)
list(APPEND plugin_libs rbs_object_detection)
add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
list(APPEND plugin_libs rbs_env_manager_starter)

View file

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<root main_tree_to_execute="ObjDetection">
<BehaviorTree ID="ObjDetection">
<Sequence>
<Action ID="ObjectDetection"
SettingPath=""
ReqKind = "deactivate"
server_name="/object_detection/change_state"
server_timeout="1000"/>
<Action ID="ObjectDetection"
SettingPath=""
ReqKind = "cleanup"
server_name="/object_detection/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

View file

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<root main_tree_to_execute="ObjDetection">
<BehaviorTree ID="ObjDetection">
<Sequence>
<Action ID="ObjectDetection"
SettingPath="!/home/shalenikol/0yolov8/test.json"
ReqKind = "configure"
server_name="/object_detection/change_state"
server_timeout="1000"/>
<Action ID="ObjectDetection"
SettingPath=""
ReqKind = "activate"
server_name="/object_detection/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

View file

@ -7,6 +7,7 @@ bt_engine:
"rbs_skill_move_joint_state",
"rbs_add_planning_scene_object",
"rbs_pose_estimation",
"rbs_object_detection",
"rbs_env_manager_starter",
"rbs_skill_move_topose_array_bt_action_client"
]

View file

@ -0,0 +1,119 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/logging.hpp>
#include <string>
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
class ObjectDetection : public BtService<ObjDetectionSrv> {
public:
ObjectDetection(const std::string &name, const BT::NodeConfiguration &config)
: BtService<ObjDetectionSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
_node, "/object_detection");
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...");
}
_setting_path = getInput<std::string>("SettingPath").value();
}
ObjDetectionSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<ObjDetectionSrv::Request>();
_req_type = getInput<std::string>("ReqKind").value();
request->set__transition(transition_event(_req_type));
return request;
}
BT::NodeStatus
handle_response(const ObjDetectionSrv::Response::SharedPtr response) {
if (response->success) {
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("ReqKind"),
BT::InputPort<std::string>("SettingPath"),
});
}
private:
uint8_t transition_id_{};
std::string _setting_path{};
// std::string _model_path{};
std::string _req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::vector<rcl_interfaces::msg::Parameter> _params;
rcl_interfaces::msg::Parameter _param;
lifecycle_msgs::msg::Transition
transition_event(const std::string &req_type) {
lifecycle_msgs::msg::Transition translation{};
if (req_type == "configure") {
set_str_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "activate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
} else if (req_type == "deactivate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
} else if (req_type == "cleanup") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
}
translation.set__id(transition_id_);
return translation;
}
// inline std::string build_model_path(const std::string &model_name,
// const std::string &package_path) {
// return package_path + "/config/" + model_name + ".ply";
// }
// inline std::string build_model_path(const std::string &model_path) {
// return model_path;
// }
void set_str_param() {
RCLCPP_INFO_STREAM(_node->get_logger(),
"Set string parameter: <" + _setting_path + ">");
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("setting_path", _setting_path)};
_set_params_client->set_parameters(_parameters);
}
// void set_all_param() {
// auto _package_name =
// ament_index_cpp::get_package_share_directory("rbs_perception");
// _model_path = build_model_path(_setting_path, _package_name);
// RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
// std::vector<rclcpp::Parameter> _parameters{
// rclcpp::Parameter("setting_path", _setting_path)};
// _set_params_client->set_parameters(_parameters);
// }
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<ObjectDetection>("ObjectDetection");
}