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

Merge branch 'main' into fix-simulation-running

Browse source
This commit is contained in:
Ilya Uraev 2024-05-27 14:21:22 +03:00
commit 38401d9eff
15 changed files with 536 additions and 124 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rbs_bt_executor/bt_trees/bt_pe_stop.xml
View file

@ -2,12 +2,13 @@
<root main_tree_to_execute="Main">
<BehaviorTree ID="Main">
<Sequence>
<Action ID="RbsBtAction" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
<Action ID="RbsBtAction" sid="124" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="RbsBtAction">
<input_port name="sid" />
<input_port name="do" />
<input_port name="command" />
<input_port name="server_name" />

3
rbs_bt_executor/bt_trees/bt_pe_test.xml
View file

@ -2,12 +2,13 @@
<root main_tree_to_execute="Main">
<BehaviorTree ID="Main">
<Sequence>
<Action ID="RbsBtAction" do="PoseEstimation" command="peConfigure" server_name="rbs_interface" server_timeout="1000" />
<Action ID="RbsBtAction" sid="123" do="PoseEstimation" command="peConfigure" server_name="rbs_interface" server_timeout="1000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="RbsBtAction">
<input_port name="sid" />
<input_port name="do" />
<input_port name="command" />
<input_port name="server_name" />

59
rbs_bt_executor/launch/rbs_bt_web.launch.py Normal file
View file

@ -0,0 +1,59 @@
"""
rbs_bt_web
ROS 2 launch program for Robossembler
@shalenikol release 0.1
"""
import os
# import json
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
from launch.substitutions import LaunchConfiguration
from launch.event_handlers import OnExecutionComplete
FILE_BT = "bt.xml"
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
bt_path = LaunchConfiguration("bt_path")
bt_path = bt_path.perform(context)
rbs_bt = Node(
package = "behavior_tree",
executable = "bt_engine",
parameters = [
{"bt_file_path": os.path.join(bt_path, FILE_BT)},
{"plugins": ["rbs_interface"]}
]
)
bt_param = Node(
package="rbs_interface",
executable="bt_param.py",
parameters = [{"bt_path": bt_path}]
)
# nodes_to_start = [rbs_bt]
# return nodes_to_start
# return [bt_param]
return [
RegisterEventHandler(
event_handler=OnExecutionComplete(
target_action=bt_param,
on_completion=[rbs_bt],
)
),
bt_param
] # the order of elements is strictly required
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"bt_path",
default_value="''",
description="path to Behavior tree instance"
)
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

46
rbs_bt_executor/src/rbsBTAction.cpp
View file

@ -1,9 +1,11 @@
// #include "behavior_tree/BtAction.hpp"
#include "behavior_tree/BtService.hpp"
// #include "rbs_skill_interfaces/action/rbs_bt.hpp"
#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
// using RbsBtActionSrv = rbs_skill_interfaces::action::RbsBt;
#define STR_ACTION "do"
#define STR_SID "sid"
#define STR_COMMAND "command"
#define NODE_NAME "rbs_interface"
using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt;
class RbsBtAction : public BtService<RbsBtActionSrv> {
@ -11,45 +13,30 @@ public:
RbsBtAction(const std::string &name, const BT::NodeConfiguration &config)
: BtService<RbsBtActionSrv>(name, config) {
_action_name = getInput<std::string>("do").value();
_action_name = getInput<std::string>(STR_ACTION).value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name);
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "rbs_interface");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, NODE_NAME);
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(), "'rbs_interface' service not available, waiting again...");
RCLCPP_WARN(_node->get_logger(), NODE_NAME " service not available, waiting again...");
}
// set_str_param();
}
// RbsBtActionSrv::Goal populate_goal() override {
// auto goal = RbsBtActionSrv::Goal();
// goal.action = _action_name;
// goal.command = getInput<std::string>("command").value();
// RCLCPP_INFO_STREAM(_node->get_logger(), "Goal send " + _action_name);
// return goal;
// }
RbsBtActionSrv::Request::SharedPtr populate_request() override {
auto request = std::make_shared<RbsBtActionSrv::Request>();
request->action = _action_name;
request->command = getInput<std::string>("command").value();
// RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:populate_request");
request->sid = getInput<std::string>(STR_SID).value();
request->command = getInput<std::string>(STR_COMMAND).value();
return request;
}
BT::NodeStatus handle_response(const RbsBtActionSrv::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);
// RCLCPP_INFO_STREAM(_node->get_logger(), "RbsBtAction:handle_response");
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
@ -57,22 +44,15 @@ public:
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("do"),
BT::InputPort<std::string>("command")
BT::InputPort<std::string>(STR_SID),
BT::InputPort<std::string>(STR_ACTION),
BT::InputPort<std::string>(STR_COMMAND)
});
}
private:
std::string _action_name{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
// void set_str_param() {
// RCLCPP_INFO_STREAM(_node->get_logger(),"Set string parameter: <" + _action_name + ">");
// std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("action_name", _action_name)};
// _set_params_client->set_parameters(_parameters);
// }
// auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
};
#include "behaviortree_cpp_v3/bt_factory.h"