Merge branch 'main' into 77-cartesian-controllers
This commit is contained in:
commit
9c56582d9f
12 changed files with 582 additions and 7 deletions
|
@ -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)
|
||||
|
||||
|
|
17
rbs_bt_executor/bt_trees/test_objdet_cleanup.xml
Normal file
17
rbs_bt_executor/bt_trees/test_objdet_cleanup.xml
Normal 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>
|
17
rbs_bt_executor/bt_trees/test_objdet_run.xml
Normal file
17
rbs_bt_executor/bt_trees/test_objdet_run.xml
Normal 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>
|
|
@ -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"
|
||||
]
|
||||
|
|
119
rbs_bt_executor/src/ObjectDetection.cpp
Normal file
119
rbs_bt_executor/src/ObjectDetection.cpp
Normal 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");
|
||||
}
|
Loading…
Add table
Add a link
Reference in a new issue