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

add workspace inspector skill

Browse source
This commit is contained in:
Ilya Uraev 2023-12-30 23:42:10 +03:00
parent 4f0c43bee9
commit 9efab819af
9 changed files with 158 additions and 85 deletions
Download patch file Download diff file Expand all files Collapse all files

10
rbs_bt_executor/src/EnvManager.cpp
View file

@ -2,9 +2,11 @@
#include <behaviortree_cpp_v3/basic_types.h>
#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>
#include "rbs_utils/rbs_utils.hpp"
#include "env_manager_interfaces/srv/start_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"
@ -31,6 +33,11 @@ public:
BT::NodeStatus handle_response(
const EnvStarterService::Response::SharedPtr response) override {
if (response->ok) {
// TODO We need better perfomance for call it in another place for all BT nodes
// - Make it via shared_ptr forward throught blackboard
auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
auto p = t->getWorkspaceInspectorTrajectory();
setOutput("workspace", p);
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
@ -40,6 +47,7 @@ public:
return providedBasicPorts({
BT::InputPort<std::string>("env_name"),
BT::InputPort<std::string>("env_class"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("workspace"),
});
}
};