Cartesian Move_to_pose Skill for BT Interface Node
This commit is contained in:
parent
bf965bb750
commit
b4b452297d
18 changed files with 743 additions and 64 deletions
|
|
@ -88,8 +88,8 @@ list(APPEND plugin_libs rbs_get_workspace)
|
|||
add_executable(rbs_bt_executor src/TreeRunner.cpp)
|
||||
ament_target_dependencies(rbs_bt_executor ${dependencies})
|
||||
|
||||
# add_library(rbs_interface SHARED src/rbsBTAction.cpp)
|
||||
# list(APPEND plugin_libs rbs_interface)
|
||||
add_library(rbs_act SHARED src/rbsBTAction.cpp)
|
||||
list(APPEND plugin_libs rbs_act)
|
||||
|
||||
foreach(bt_plugin ${plugin_libs})
|
||||
ament_target_dependencies(${bt_plugin} ${dependencies})
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@
|
|||
ROS 2 launch program for Robossembler
|
||||
|
||||
@shalenikol release 0.1
|
||||
@shalenikol release 0.2 BT v.4
|
||||
"""
|
||||
import os
|
||||
# import json
|
||||
|
|
@ -13,35 +14,50 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventH
|
|||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.event_handlers import OnExecutionComplete
|
||||
|
||||
FILE_BT = "bt.xml"
|
||||
# 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"]}
|
||||
# ]
|
||||
# )
|
||||
rbs_bt = Node(
|
||||
package = "behavior_tree",
|
||||
executable = "bt_engine",
|
||||
package = "rbs_bt_executor",
|
||||
executable = "rbs_bt_executor",
|
||||
parameters = [
|
||||
{"bt_file_path": os.path.join(bt_path, FILE_BT)},
|
||||
{"plugins": ["rbs_interface"]}
|
||||
{
|
||||
"plugins": ["rbs_bt_executor/bt_plugins"]
|
||||
# "behavior_trees": [bt_path]
|
||||
}
|
||||
]
|
||||
)
|
||||
|
||||
bt_exec = Node(
|
||||
package="rbs_interface",
|
||||
executable="bt_exec",
|
||||
arguments=[bt_path]
|
||||
# prefix=['gdbserver localhost:3000'],
|
||||
)
|
||||
|
||||
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],
|
||||
on_completion=[bt_exec],
|
||||
# on_completion=[rbs_bt, bt_exec],
|
||||
)
|
||||
),
|
||||
bt_param
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@ public:
|
|||
}
|
||||
|
||||
bool setRequest(Request::SharedPtr &request) override {
|
||||
request->action = m_action_name;
|
||||
getInput(STR_SID, request->sid);
|
||||
getInput(STR_COMMAND, request->command);
|
||||
return true;
|
||||
|
|
@ -59,4 +60,5 @@ private:
|
|||
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
|
||||
};
|
||||
|
||||
CreateRosNodePlugin(RbsBtAction, "RbsAction")
|
||||
// !!! теперь устаревшая версия !!!
|
||||
CreateRosNodePlugin(RbsBtAction, "RbsBtAction")
|
||||
|
|
|
|||
|
|
@ -19,9 +19,11 @@ find_package(rbs_skill_interfaces REQUIRED)
|
|||
find_package(PCL 1.12 REQUIRED)
|
||||
find_package(pcl_conversions REQUIRED)
|
||||
find_package(pcl_ros REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_msgs REQUIRED)
|
||||
# find_package(tf2_ros REQUIRED)
|
||||
# find_package(tf2_msgs REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
find_package(behaviortree_ros2 REQUIRED)
|
||||
find_package(btcpp_ros2_interfaces REQUIRED)
|
||||
|
||||
# Install Python modules
|
||||
ament_python_install_package(${PROJECT_NAME})
|
||||
|
|
@ -33,6 +35,21 @@ install(PROGRAMS
|
|||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
set(THIS_PACKAGE_DEPS
|
||||
rbs_skill_interfaces
|
||||
behaviortree_ros2
|
||||
# std_msgs
|
||||
# std_srvs
|
||||
btcpp_ros2_interfaces )
|
||||
|
||||
add_executable(bt_exec src/BTExec.cpp)
|
||||
ament_target_dependencies(bt_exec ${THIS_PACKAGE_DEPS})
|
||||
|
||||
install(TARGETS
|
||||
bt_exec
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@ def get_skill_list_(path: str) -> list:
|
|||
excluding = {}
|
||||
skills = []
|
||||
for skill in data["skills"]:
|
||||
node_name = skill["Module"]["node_name"]
|
||||
launch = skill["Launch"]
|
||||
p = launch["package"]
|
||||
e = launch["executable"]
|
||||
|
|
@ -37,7 +38,7 @@ def get_skill_list_(path: str) -> list:
|
|||
continue
|
||||
nn_skills += 1
|
||||
# skills.append(Node(package=p, executable=e))
|
||||
launch["parameters"] = [{launch["name"]+PARAM_SUFFIX: json.dumps(skill)}]
|
||||
launch["parameters"] = [{node_name+PARAM_SUFFIX: json.dumps(skill)}]
|
||||
skills.append(Node(**launch))
|
||||
|
||||
return skills
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>rbs_skill_interfaces</depend>
|
||||
<depend>behaviortree_ros2</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
|
|
|||
|
|
@ -4,44 +4,54 @@
|
|||
ROS 2 program for Robossembler
|
||||
|
||||
@shalenikol release 0.1
|
||||
@shalenikol release 0.2 BT v.4
|
||||
"""
|
||||
import os
|
||||
import json
|
||||
# import yaml
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
# from rclpy.action import ActionClient
|
||||
from rclpy.action import ActionServer
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from rclpy.parameter import Parameter
|
||||
from rcl_interfaces.srv import SetParameters #, GetParameters
|
||||
from rcl_interfaces.msg import SetParametersResult #, ParameterEvent
|
||||
from lifecycle_msgs.srv import ChangeState #, GetState
|
||||
from lifecycle_msgs.msg import Transition
|
||||
from lifecycle_msgs.srv import ChangeState, GetState
|
||||
from lifecycle_msgs.msg import Transition, State
|
||||
from rbs_skill_interfaces.srv import RbsBt
|
||||
from rbs_skill_interfaces.action import RbsBt as RbsBtAction
|
||||
# from btcpp_ros2_interfaces.action import ExecuteTree
|
||||
|
||||
# from rclpy.parameter_client import AsyncParameterClient # only Iron
|
||||
|
||||
# PARAM_NAME = "str_param"
|
||||
# PARAM_SKILL_CONFIG = "skill_cfg"
|
||||
PARAM_BT = "bt_path"
|
||||
# BT_SERVER = "bt_execution"
|
||||
CONDITION_SRV_NAME = "/condition"
|
||||
BT_PARAM = "bt_path"
|
||||
NODE_NAME = "rbs_interface"
|
||||
SERVICE_NAME = "rbs_interface_s"
|
||||
SERVER_NAME = "rbs_interface_a"
|
||||
FILE_SKILLS = "skills.json"
|
||||
PARAM_SUFFIX = "_cfg"
|
||||
|
||||
# def get_transfer_path_():
|
||||
# return os.path.join(get_package_share_directory("rbs_interface"), "config")
|
||||
KEY_BTPARAM = "BTAction" # TODO "bt_param"
|
||||
|
||||
class rbsInterface(Node):
|
||||
def __init__(self, node_name):
|
||||
"""Construct the node."""
|
||||
self.bt_path = "" # path to the current BehaviorTree
|
||||
self.cfg_data = None # config for current action
|
||||
self._timer = None # for BT v.4
|
||||
super().__init__(node_name)
|
||||
self.declare_parameter(PARAM_BT, rclpy.Parameter.Type.STRING)
|
||||
self.declare_parameter(BT_PARAM, rclpy.Parameter.Type.STRING)
|
||||
self.cb_group = ReentrantCallbackGroup()
|
||||
self._service = self.create_service(RbsBt, node_name, self.service_callback, callback_group=self.cb_group)
|
||||
# for Condition
|
||||
self._service = self.create_service(RbsBt, SERVICE_NAME, self.service_callback, callback_group=self.cb_group)
|
||||
# for Action
|
||||
self._action = ActionServer(self, RbsBtAction, SERVER_NAME, self.action_callback, callback_group=self.cb_group)
|
||||
# self.bt_client = ActionClient(self, ExecuteTree, BT_SERVER, callback_group=self.cb_group)
|
||||
|
||||
# self.client = AsyncParameterClient(self.client_node, 'test_parameter_client_target') # only Iron
|
||||
self.add_on_set_parameters_callback(self._on_set_btpath_param)
|
||||
|
||||
|
|
@ -52,12 +62,11 @@ class rbsInterface(Node):
|
|||
|
||||
def _on_set_btpath_param(self, parameter_list):
|
||||
for parameter in parameter_list:
|
||||
if parameter.name == PARAM_BT:
|
||||
if parameter.name == BT_PARAM:
|
||||
self.bt_path = parameter.value
|
||||
# self.get_logger().info(f'$ {parameter.name}={parameter.value}')
|
||||
return SetParametersResult(successful=True)
|
||||
|
||||
|
||||
# def get_remote_parameter(self, remote_node_name, param_name):
|
||||
# cli = self.create_client(GetParameters, remote_node_name + '/get_parameters')
|
||||
# while not cli.wait_for_service(timeout_sec=1.0):
|
||||
|
|
@ -86,7 +95,7 @@ class rbsInterface(Node):
|
|||
|
||||
self.executor.spin_until_future_complete(future)
|
||||
res = future.result()
|
||||
|
||||
# self.get_logger().info(f"{res}")
|
||||
return res.results[0].successful
|
||||
|
||||
def _deserialize(self, file_path: str, sid: str):
|
||||
|
|
@ -109,18 +118,11 @@ class rbsInterface(Node):
|
|||
def run_action(self, command_data: dict) -> bool:
|
||||
p_list = command_data["param"]
|
||||
oper_type = command_data["type"]
|
||||
node_name = self.cfg_data["Launch"]["name"] #["ROS2"]["node_name"]
|
||||
node_name = self.cfg_data["Module"]["node_name"]
|
||||
par_name = node_name + PARAM_SUFFIX
|
||||
|
||||
if len(p_list) > 0:
|
||||
# ext = command_data["format"] # 'yaml' or 'json'
|
||||
# param_file = os.path.join(self.get_transfer_path(), command_data["name"]+"."+ext)
|
||||
# with open(param_file, "r") as f:
|
||||
# data = f.read()
|
||||
# if not self.set_remote_parameter(node_name, par_name, data):
|
||||
# return False
|
||||
# if not self.set_remote_parameter(node_name, par_name, yaml.dump(self.cfg_data)):
|
||||
data = json.dumps(self.cfg_data)
|
||||
# self.get_logger().info(f"{data}")
|
||||
if not self.set_remote_parameter(node_name, par_name, data):
|
||||
return False
|
||||
|
||||
|
|
@ -165,19 +167,53 @@ class rbsInterface(Node):
|
|||
res = future.result()
|
||||
if res: # is not None:
|
||||
ret = res.success
|
||||
|
||||
elif oper_type == "action":
|
||||
ret = True
|
||||
|
||||
return ret
|
||||
|
||||
def run_condition(self, command_data: dict) -> bool:
|
||||
node_name = self.cfg_data["Module"]["node_name"]
|
||||
|
||||
self.cli = self.create_client(RbsBt, node_name + CONDITION_SRV_NAME)
|
||||
while not self.cli.wait_for_service(timeout_sec=1.0):
|
||||
self.get_logger().info("'" + node_name + "' service not available, waiting again...")
|
||||
req = RbsBt.Request()
|
||||
req.command = command_data["name"]
|
||||
future = self.cli.call_async(req)
|
||||
|
||||
self.executor.spin_until_future_complete(future)
|
||||
res = future.result()
|
||||
return res.ok
|
||||
|
||||
def _interface(self, sid: str, action: str, command: str, isCondition: bool) -> bool:
|
||||
self.get_logger().info(f"Incoming request ({action}/{command})")
|
||||
self.cfg_data = self._load_config(sid)
|
||||
typeBTnode = "Condition" if isCondition else "Action"
|
||||
self.get_logger().info(f'{typeBTnode}: Ok ({self.cfg_data["Module"]["description"]})')
|
||||
|
||||
is_success = False
|
||||
for comm in self.cfg_data[KEY_BTPARAM]:
|
||||
if comm["name"] == command:
|
||||
is_success = self.run_condition(comm) if isCondition else self.run_action(comm)
|
||||
|
||||
return is_success
|
||||
|
||||
def action_callback(self, goal_h):
|
||||
goal = goal_h.request
|
||||
is_success = self._interface(goal.sid, goal.action, goal.command, isCondition=False)
|
||||
res = RbsBtAction.Result()
|
||||
res.ok = is_success
|
||||
|
||||
if is_success:
|
||||
goal_h.succeed()
|
||||
else:
|
||||
goal_h.abort()
|
||||
return res
|
||||
|
||||
def service_callback(self, request, response):
|
||||
self.get_logger().info(f"Incoming request for Action ({request.action}/{request.command})")
|
||||
self.cfg_data = self._load_config(request.sid) #, request.command)
|
||||
self.get_logger().info(f'Config: Ok ({self.cfg_data["Module"]["description"]})')
|
||||
|
||||
is_action = False
|
||||
for comm in self.cfg_data["BTAction"]:
|
||||
if comm["name"] == request.command:
|
||||
is_action = self.run_action(comm)
|
||||
|
||||
response.ok = is_action #True
|
||||
response.ok = self._interface(request.sid, request.action, request.command, isCondition=True)
|
||||
return response
|
||||
|
||||
def main():
|
||||
|
|
|
|||
201
rbs_interface/src/BTExec.cpp
Normal file
201
rbs_interface/src/BTExec.cpp
Normal file
|
|
@ -0,0 +1,201 @@
|
|||
#include <behaviortree_cpp/tree_node.h>
|
||||
#include <behaviortree_ros2/plugins.hpp>
|
||||
#include <behaviortree_ros2/ros_node_params.hpp>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include "behaviortree_ros2/bt_service_node.hpp"
|
||||
#include "behaviortree_ros2/bt_action_node.hpp"
|
||||
#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
|
||||
#include "rbs_skill_interfaces/action/rbs_bt.hpp"
|
||||
|
||||
#define STR_ACTION "do"
|
||||
#define STR_SID "sid"
|
||||
#define STR_COMMAND "command"
|
||||
#define NODE_NAME "rbs_interface"
|
||||
#define SERVICE_NAME "rbs_interface_s"
|
||||
#define SERVER_NAME "rbs_interface_a"
|
||||
#define FILE_BT "bt.xml"
|
||||
|
||||
template<typename T> std::string to_string(const T &t)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << t;
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
using namespace BT;
|
||||
using namespace std::chrono_literals;
|
||||
using RbsConditionSrv = rbs_skill_interfaces::srv::RbsBt;
|
||||
|
||||
using RbsActionSrv = rbs_skill_interfaces::action::RbsBt;
|
||||
|
||||
class RbsAction : public RosActionNode<RbsActionSrv> {
|
||||
public:
|
||||
RbsAction(const std::string &name, const NodeConfig &conf, const RosNodeParams ¶ms)
|
||||
: RosActionNode<RbsActionSrv>(name, conf, params) {
|
||||
|
||||
m_action = getInput<std::string>(STR_ACTION).value();
|
||||
m_command = getInput<std::string>(STR_COMMAND).value();
|
||||
RCLCPP_INFO_STREAM(logger(), "Start node RbsAction: " + m_action + "/" + m_command);
|
||||
|
||||
m_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
|
||||
|
||||
while (!m_client->wait_for_service(1s)) {
|
||||
if (!rclcpp::ok()) {
|
||||
RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
|
||||
break;
|
||||
}
|
||||
RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return providedBasicPorts({
|
||||
BT::InputPort<std::string>(STR_SID),
|
||||
BT::InputPort<std::string>(STR_ACTION),
|
||||
BT::InputPort<std::string>(STR_COMMAND)
|
||||
});
|
||||
}
|
||||
|
||||
bool setGoal(RosActionNode::Goal& goal) override {
|
||||
getInput(STR_ACTION, goal.action);
|
||||
getInput(STR_COMMAND, goal.command);
|
||||
getInput(STR_SID, goal.sid);
|
||||
m_action = goal.action;
|
||||
m_command = goal.command;
|
||||
|
||||
m_i++;
|
||||
RCLCPP_INFO_STREAM(logger(), "setGoal: " + to_string(m_i) + ") " + goal.action + "/" + goal.command + " goal.sid = " + to_string(goal.sid));
|
||||
return true;
|
||||
}
|
||||
|
||||
NodeStatus onResultReceived(const WrappedResult& wr) override {
|
||||
m_i++;
|
||||
RCLCPP_INFO_STREAM(logger(), "onResultReceived: " + to_string(m_i) + ") " + m_action + "/" + m_command + " wr.result->ok - " + to_string(wr.result->ok));
|
||||
|
||||
if (!wr.result->ok) {
|
||||
return NodeStatus::FAILURE;
|
||||
}
|
||||
return NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
private:
|
||||
int m_i{0};
|
||||
std::string m_action{};
|
||||
std::string m_command{};
|
||||
std::shared_ptr<rclcpp::AsyncParametersClient> m_client;
|
||||
};
|
||||
|
||||
// **********************************************************
|
||||
class RbsCondition : public RosServiceNode<RbsConditionSrv> {
|
||||
public:
|
||||
RbsCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
|
||||
: RosServiceNode<RbsConditionSrv>(name, conf, params) {
|
||||
|
||||
m_action_name = getInput<std::string>(STR_ACTION).value();
|
||||
m_command = getInput<std::string>(STR_COMMAND).value();
|
||||
RCLCPP_INFO_STREAM(logger(), "Start node RbsCondition: " + m_action_name + "/" + m_command);
|
||||
|
||||
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
|
||||
|
||||
while (!m_params_client->wait_for_service(1s)) {
|
||||
if (!rclcpp::ok()) {
|
||||
RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
|
||||
break;
|
||||
}
|
||||
RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
|
||||
}
|
||||
}
|
||||
|
||||
bool setRequest(Request::SharedPtr &request) override {
|
||||
request->action = m_action_name;
|
||||
request->command = m_command;
|
||||
getInput(STR_SID, request->sid);
|
||||
|
||||
m_i++;
|
||||
RCLCPP_INFO_STREAM(logger(), "setRequest: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " request->sid = " + to_string(request->sid));
|
||||
return true;
|
||||
}
|
||||
|
||||
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
||||
m_i++;
|
||||
RCLCPP_INFO_STREAM(logger(), "onResponseReceived: " + to_string(m_i) + ") " + m_action_name + "/" + m_command + " response->ok - " + to_string(response->ok));
|
||||
|
||||
if (!response->ok) {
|
||||
return NodeStatus::FAILURE;
|
||||
}
|
||||
return NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
static PortsList providedPorts() {
|
||||
return providedBasicPorts({
|
||||
InputPort<std::string>(STR_SID),
|
||||
InputPort<std::string>(STR_ACTION),
|
||||
InputPort<std::string>(STR_COMMAND)
|
||||
});
|
||||
}
|
||||
|
||||
private:
|
||||
int m_i{0};
|
||||
std::string m_action_name{};
|
||||
std::string m_command{};
|
||||
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
|
||||
};
|
||||
|
||||
// class RbsCondition : public RosTopicSubNode {
|
||||
// }
|
||||
// Simple function that return a NodeStatus
|
||||
// BT::NodeStatus TestIf()
|
||||
// {
|
||||
// std::string m_if = "if"; //getInput<std::string>("if").value();
|
||||
// // std::cout << "[ Test: OK ]" << std::endl;
|
||||
|
||||
// std::stringstream ss;
|
||||
// ss << "[ Test: OK ] " << m_if;
|
||||
// std::string str = ss.str();
|
||||
// std::cout << str << std::endl;
|
||||
// return BT::NodeStatus::SUCCESS;
|
||||
// }
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
// filename Behavior Tree
|
||||
std::string bt = std::string(argv[1]) + "/" FILE_BT;
|
||||
std::ifstream fh(bt, std::ios::in);
|
||||
// reading xml
|
||||
std::string xml{std::istreambuf_iterator<char>(fh), std::istreambuf_iterator<char>()};
|
||||
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), xml.c_str());
|
||||
auto nh = std::make_shared<rclcpp::Node>("bt_exec"); //"_action");
|
||||
// auto nh2 = std::make_shared<rclcpp::Node>("bt_exe_condition");
|
||||
|
||||
RosNodeParams params; //,params2;
|
||||
params.nh = nh;
|
||||
params.default_port_value = SERVICE_NAME;
|
||||
params.server_timeout = 3s;
|
||||
|
||||
BehaviorTreeFactory factory;
|
||||
factory.registerNodeType<RbsCondition>("Is", params);
|
||||
|
||||
// params.nh = nh;
|
||||
params.default_port_value = SERVER_NAME;
|
||||
// params.server_timeout = 3s;
|
||||
factory.registerNodeType<RbsAction>("RbsAction", params);
|
||||
// factory.registerSimpleCondition("Is", [&](TreeNode&) { return TestIf(); });
|
||||
// factory.registerRosAction("Is", params);
|
||||
|
||||
auto tree = factory.createTreeFromText(xml);
|
||||
|
||||
// while(rclcpp::ok())
|
||||
if(rclcpp::ok())
|
||||
{
|
||||
tree.tickWhileRunning();
|
||||
}
|
||||
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
|||
"action/MoveitSendPose.action"
|
||||
"action/MoveitSendJointStates.action"
|
||||
"action/GripperCommand.action"
|
||||
"action/RbsBt.action"
|
||||
"msg/ObjectInfo.msg"
|
||||
"msg/PropertyValuePair.msg"
|
||||
"msg/ActionFeedbackStatusConstants.msg"
|
||||
|
|
|
|||
11
rbs_skill_interfaces/action/RbsBt.action
Normal file
11
rbs_skill_interfaces/action/RbsBt.action
Normal file
|
|
@ -0,0 +1,11 @@
|
|||
string sid
|
||||
string action
|
||||
string command
|
||||
---
|
||||
#result definition
|
||||
bool ok
|
||||
string status #Use the constants of ActionResultStatusConstants in the status field
|
||||
---
|
||||
#feedback
|
||||
bool ok
|
||||
string status #Use the constants of ActionFeedbackStatusConstants in the status field
|
||||
|
|
@ -67,15 +67,15 @@ def launch_setup(context, *args, **kwargs):
|
|||
]
|
||||
)
|
||||
|
||||
cartesian_move_to_pose_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
executable="move_to_pose.py",
|
||||
namespace=namespace,
|
||||
parameters=[
|
||||
{"use_sim_time": use_sim_time},
|
||||
{"robot_name": namespace}
|
||||
]
|
||||
)
|
||||
# cartesian_move_to_pose_action_server = Node(
|
||||
# package="rbs_skill_servers",
|
||||
# executable="move_to_pose.py",
|
||||
# namespace=namespace,
|
||||
# parameters=[
|
||||
# {"use_sim_time": use_sim_time},
|
||||
# {"robot_name": namespace}
|
||||
# ]
|
||||
# )
|
||||
|
||||
move_joint_state_action_server = Node(
|
||||
package="rbs_skill_servers",
|
||||
|
|
@ -101,7 +101,7 @@ def launch_setup(context, *args, **kwargs):
|
|||
gripper_control_node,
|
||||
move_cartesian_path_action_server,
|
||||
move_joint_state_action_server,
|
||||
cartesian_move_to_pose_action_server,
|
||||
# cartesian_move_to_pose_action_server,
|
||||
# grasp_pose_loader
|
||||
]
|
||||
return nodes_to_start
|
||||
|
|
|
|||
45
rbss_movetopose/CMakeLists.txt
Normal file
45
rbss_movetopose/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,45 @@
|
|||
cmake_minimum_required(VERSION 3.8)
|
||||
project(rbss_movetopose)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(ament_cmake_python REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(rbs_skill_interfaces REQUIRED)
|
||||
find_package(PCL 1.12 REQUIRED)
|
||||
find_package(pcl_conversions REQUIRED)
|
||||
find_package(pcl_ros REQUIRED)
|
||||
|
||||
# Install Python modules
|
||||
ament_python_install_package(${PROJECT_NAME})
|
||||
|
||||
# Install Python executables
|
||||
install(PROGRAMS
|
||||
scripts/mtp_cartesian.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY launch
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
17
rbss_movetopose/launch/movetopose.launch.py
Normal file
17
rbss_movetopose/launch/movetopose.launch.py
Normal file
|
|
@ -0,0 +1,17 @@
|
|||
# from launch_ros.actions import Node
|
||||
# from launch import LaunchDescription
|
||||
|
||||
def generate_launch_description():
|
||||
print("The skill must be launched via interface node")
|
||||
return []
|
||||
# declared_arguments = []
|
||||
|
||||
# rbss_od = Node(
|
||||
# package="rbss_movetopose",
|
||||
# executable="movetopose.py",
|
||||
# )
|
||||
|
||||
# nodes_to_start = [
|
||||
# rbss_od,
|
||||
# ]
|
||||
# return LaunchDescription(declared_arguments + nodes_to_start)
|
||||
24
rbss_movetopose/package.xml
Normal file
24
rbss_movetopose/package.xml
Normal file
|
|
@ -0,0 +1,24 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rbss_movetopose</name>
|
||||
<version>0.0.1</version>
|
||||
<description>The Move to pose skill</description>
|
||||
<maintainer email="shaniks77s@gmail.com">shalenikol</maintainer>
|
||||
<license>Apache License 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>ament_cmake_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>rbs_skill_interfaces</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
30
rbss_movetopose/rbs_package.json
Normal file
30
rbss_movetopose/rbs_package.json
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
{
|
||||
"SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" },
|
||||
"Module": { "name": "MoveToPose", "description": "Move to Pose skill with cartesian controllers" },
|
||||
"Launch": { "package": "rbss_movetopose", "executable": "mtp_cartesian.py", "name": "mtp_cartesian" },
|
||||
"BTAction": [
|
||||
{
|
||||
"name": "move",
|
||||
"type": "action",
|
||||
"param": [
|
||||
{
|
||||
"type": "move_to_pose",
|
||||
"dependency": { "robot_name": "arm0",
|
||||
"pose": { "position": {"x":0.0, "y":0.0, "z":0.0}, "orientation": {"x":0.0, "y":0.0, "z":0.0, "w": 1.0} } }
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
"Interface": {
|
||||
"Input": [
|
||||
{ "name": "robotName", "type": "ROBOT" },
|
||||
{ "name": "pose", "type": "POSE" }
|
||||
],
|
||||
"Output": []
|
||||
},
|
||||
"Settings": [
|
||||
{ "name": "server_name", "value": "cartesian_move_to_pose" },
|
||||
{ "name": "end_effector_velocity", "value": 1.0 },
|
||||
{ "name": "end_effector_acceleration", "value": 1.0 }
|
||||
]
|
||||
}
|
||||
0
rbss_movetopose/rbss_movetopose/__init__.py
Normal file
0
rbss_movetopose/rbss_movetopose/__init__.py
Normal file
264
rbss_movetopose/scripts/mtp_cartesian.py
Executable file
264
rbss_movetopose/scripts/mtp_cartesian.py
Executable file
|
|
@ -0,0 +1,264 @@
|
|||
#!/usr/bin/env python3
|
||||
"""
|
||||
Move_to_pose_cartesian_node_via_interface_node
|
||||
ROS 2 program for Move to Pose skill
|
||||
|
||||
@shalenikol release 0.1
|
||||
"""
|
||||
import json
|
||||
import math
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Slerp, Rotation as R
|
||||
|
||||
import rclpy
|
||||
from rclpy.action import ActionClient, ActionServer
|
||||
from rclpy.node import Node
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
|
||||
from rcl_interfaces.msg import SetParametersResult
|
||||
from geometry_msgs.msg import Pose, PoseStamped
|
||||
|
||||
from rbs_skill_interfaces.action import MoveitSendPose
|
||||
|
||||
NODE_NAME_DEFAULT = "mtp_cartesian" # the name doesn't matter in this node (defined in Launch)
|
||||
PARAM_SKILL_CFG = "mtp_cartesian_cfg"
|
||||
SERVER_NAME = "cartesian_movetopose"
|
||||
TOPIC_CURRENT_POSE = "/cartesian_motion_controller/current_pose"
|
||||
TOPIC_TARGET_FRAME = "/cartesian_motion_controller/target_frame"
|
||||
SERVER_PAR1_BASE_LINK = "base_link"
|
||||
SERVER_PAR2_ROBOT_NAME = "robot_name"
|
||||
|
||||
class MoveToPoseCartesianSkill(Node):
|
||||
""" <Move to pose> skill node """
|
||||
def _Settings(self):
|
||||
# Initialization service settings
|
||||
for prop in self.skill_cfg["Settings"]:
|
||||
nam = prop["name"]
|
||||
val = prop["value"]
|
||||
if nam == "server_name":
|
||||
self.server_name = val
|
||||
elif nam == "end_effector_velocity":
|
||||
self.end_effector_velocity = val
|
||||
elif nam == "end_effector_acceleration":
|
||||
self.end_effector_acceleration = val
|
||||
|
||||
def __init__(self, **kwargs):
|
||||
self.end_effector_velocity = 1.0
|
||||
self.end_effector_acceleration = 1.0
|
||||
# for other nodes
|
||||
kwargs["allow_undeclared_parameters"] = True
|
||||
kwargs["automatically_declare_parameters_from_overrides"] = True
|
||||
super().__init__(NODE_NAME_DEFAULT, **kwargs)
|
||||
str_cfg = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value
|
||||
|
||||
self.skill_cfg = json.loads(str_cfg)
|
||||
self.nodeName = self.skill_cfg["Module"]["node_name"] #["Launch"]["name"]
|
||||
|
||||
self.server_name = SERVER_NAME #"cartesian_move_to_pose"
|
||||
self._Settings()
|
||||
|
||||
self._action_client = ActionClient(self, MoveitSendPose, self.server_name) # "/"+ robot_name +
|
||||
|
||||
self._cnt = 0
|
||||
self.add_on_set_parameters_callback(self._on_set_param)
|
||||
|
||||
def _on_set_param(self, parameter_list):
|
||||
self._cnt += 1
|
||||
for parameter in parameter_list:
|
||||
# self.get_logger().info(f'{self._cnt}$ {parameter.name}={parameter.value}')
|
||||
if parameter.name == PARAM_SKILL_CFG:
|
||||
self.skill_cfg = json.loads(parameter.value)
|
||||
|
||||
self._Settings()
|
||||
dependency = {}
|
||||
for comm in self.skill_cfg["BTAction"]:
|
||||
for par in comm["param"]:
|
||||
if par["type"] == "move_to_pose":
|
||||
dependency = par["dependency"]
|
||||
assert dependency, "no dependency"
|
||||
|
||||
self.send_goal(dependency)
|
||||
self.get_logger().info(f"{self._cnt}) dependency = {dependency}")
|
||||
break
|
||||
return SetParametersResult(successful=True)
|
||||
|
||||
def send_goal(self, dep):
|
||||
goal_msg = MoveitSendPose.Goal()
|
||||
pose = dep["pose"]
|
||||
# goal_msg.target_pose
|
||||
goal_msg.target_pose.position.x = pose["position"]["x"]
|
||||
goal_msg.target_pose.position.y = pose["position"]["y"]
|
||||
goal_msg.target_pose.position.z = pose["position"]["z"]
|
||||
|
||||
goal_msg.target_pose.orientation.x = pose["orientation"]["x"]
|
||||
goal_msg.target_pose.orientation.y = pose["orientation"]["y"]
|
||||
goal_msg.target_pose.orientation.z = pose["orientation"]["z"]
|
||||
goal_msg.target_pose.orientation.w = pose["orientation"]["w"]
|
||||
|
||||
goal_msg.robot_name = dep["robot_name"]
|
||||
goal_msg.end_effector_velocity = self.end_effector_velocity
|
||||
goal_msg.end_effector_acceleration = self.end_effector_acceleration
|
||||
|
||||
self._action_client.wait_for_server()
|
||||
|
||||
return self._action_client.send_goal_async(goal_msg)
|
||||
|
||||
class CartesianMoveToPose(Node):
|
||||
""" <Move to pose> action server """
|
||||
def __init__(self, server_name:str = SERVER_NAME):
|
||||
super().__init__("cartesian_server")
|
||||
|
||||
self.declare_parameter(SERVER_PAR1_BASE_LINK, "base_link")
|
||||
self.declare_parameter(SERVER_PAR2_ROBOT_NAME, "")
|
||||
|
||||
self._callback_group = ReentrantCallbackGroup()
|
||||
self._action_server = ActionServer(
|
||||
self,
|
||||
MoveitSendPose,
|
||||
server_name,
|
||||
self.execute_callback, callback_group=self._callback_group)
|
||||
|
||||
self.base_link: str = self.get_parameter(SERVER_PAR1_BASE_LINK).get_parameter_value().string_value
|
||||
# for multirobot setup where each robot name is a namespace
|
||||
self.robot_name: str = self.get_parameter(SERVER_PAR2_ROBOT_NAME).get_parameter_value().string_value
|
||||
self.robot_name = self.robot_name.lstrip('/').rstrip('/')
|
||||
self.robot_name = f"/{self.robot_name}" if self.robot_name else ""
|
||||
self._pub = self.create_publisher(PoseStamped,
|
||||
self.robot_name + TOPIC_TARGET_FRAME, 1,
|
||||
callback_group=self._callback_group)
|
||||
self.current_pose = None
|
||||
self.goal_tolerance = 0.05
|
||||
self.max_speed = 0.1
|
||||
self.max_acceleration = 0.05
|
||||
|
||||
def on_pose_callback(self, msg: PoseStamped):
|
||||
if isinstance(msg, PoseStamped):
|
||||
self.current_pose = msg
|
||||
|
||||
def execute_callback(self, goal_handle):
|
||||
self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
|
||||
target_pose = goal_handle.request.target_pose
|
||||
start_pose = self.current_pose.pose if self.current_pose else None
|
||||
|
||||
if start_pose is None:
|
||||
self.get_logger().error("Current pose is not available")
|
||||
goal_handle.abort()
|
||||
return MoveitSendPose.Result()
|
||||
|
||||
trajectory = self.generate_trajectory(start_pose, target_pose)
|
||||
for point in trajectory:
|
||||
tp = PoseStamped()
|
||||
tp.pose = point
|
||||
tp.header.stamp = self.get_clock().now().to_msg()
|
||||
tp.header.frame_id = self.base_link
|
||||
self._pub.publish(tp)
|
||||
rclpy.spin_once(self, timeout_sec=0.1)
|
||||
|
||||
goal_handle.succeed()
|
||||
|
||||
result = MoveitSendPose.Result()
|
||||
result.success = True
|
||||
return result
|
||||
|
||||
def generate_trajectory(self, start_pose, target_pose):
|
||||
start_position = np.array([
|
||||
start_pose.position.x,
|
||||
start_pose.position.y,
|
||||
start_pose.position.z
|
||||
])
|
||||
target_position = np.array([
|
||||
target_pose.position.x,
|
||||
target_pose.position.y,
|
||||
target_pose.position.z
|
||||
])
|
||||
|
||||
start_orientation = R.from_quat([
|
||||
start_pose.orientation.x,
|
||||
start_pose.orientation.y,
|
||||
start_pose.orientation.z,
|
||||
start_pose.orientation.w
|
||||
])
|
||||
target_orientation = R.from_quat([
|
||||
target_pose.orientation.x,
|
||||
target_pose.orientation.y,
|
||||
target_pose.orientation.z,
|
||||
target_pose.orientation.w
|
||||
])
|
||||
|
||||
distance = np.linalg.norm(target_position - start_position)
|
||||
max_speed = self.max_speed
|
||||
max_acceleration = self.max_acceleration
|
||||
|
||||
t_acc = max_speed / max_acceleration
|
||||
d_acc = 0.5 * max_acceleration * t_acc**2
|
||||
|
||||
if distance < 2 * d_acc:
|
||||
t_acc = math.sqrt(distance / max_acceleration)
|
||||
t_flat = 0
|
||||
else:
|
||||
t_flat = (distance - 2 * d_acc) / max_speed
|
||||
|
||||
total_time = 2 * t_acc + t_flat
|
||||
num_points = int(total_time * 10)
|
||||
trajectory = []
|
||||
|
||||
times = np.linspace(0, total_time, num_points + 1)
|
||||
key_rots = R.from_quat([start_orientation.as_quat(), target_orientation.as_quat()])
|
||||
slerp = Slerp([0, total_time], key_rots)
|
||||
|
||||
for t in times:
|
||||
if t < t_acc:
|
||||
fraction = 0.5 * max_acceleration * t**2 / distance
|
||||
elif t < t_acc + t_flat:
|
||||
fraction = (d_acc + max_speed * (t - t_acc)) / distance
|
||||
else:
|
||||
t_decel = t - t_acc - t_flat
|
||||
fraction = (d_acc + max_speed * t_flat + 0.5 * max_acceleration * t_decel**2) / distance
|
||||
|
||||
intermediate_position = start_position + fraction * (target_position - start_position)
|
||||
intermediate_orientation = slerp([t])[0]
|
||||
|
||||
intermediate_pose = Pose()
|
||||
intermediate_pose.position.x = intermediate_position[0]
|
||||
intermediate_pose.position.y = intermediate_position[1]
|
||||
intermediate_pose.position.z = intermediate_position[2]
|
||||
intermediate_orientation_quat = intermediate_orientation.as_quat()
|
||||
intermediate_pose.orientation.x = intermediate_orientation_quat[0]
|
||||
intermediate_pose.orientation.y = intermediate_orientation_quat[1]
|
||||
intermediate_pose.orientation.z = intermediate_orientation_quat[2]
|
||||
intermediate_pose.orientation.w = intermediate_orientation_quat[3]
|
||||
trajectory.append(intermediate_pose)
|
||||
|
||||
return trajectory
|
||||
|
||||
class PoseSubscriber(Node):
|
||||
def __init__(self, parent: CartesianMoveToPose, robot_name: str):
|
||||
super().__init__("pose_subscriber")
|
||||
self.parent = parent
|
||||
self._sub = self.create_subscription(PoseStamped,
|
||||
robot_name + TOPIC_CURRENT_POSE,
|
||||
self.parent.on_pose_callback, 1,
|
||||
callback_group=self.parent._callback_group)
|
||||
self.get_logger().info("PoseSubscriber node initialized")
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
node = MoveToPoseCartesianSkill()
|
||||
cartesian_mtp_node = CartesianMoveToPose(node.server_name)
|
||||
pose_subscriber = PoseSubscriber(parent=cartesian_mtp_node,
|
||||
robot_name=cartesian_mtp_node.robot_name)
|
||||
|
||||
executor = rclpy.executors.MultiThreadedExecutor()
|
||||
executor.add_node(cartesian_mtp_node)
|
||||
executor.add_node(pose_subscriber)
|
||||
executor.add_node(node)
|
||||
try:
|
||||
executor.spin()
|
||||
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
|
||||
pose_subscriber.destroy_node()
|
||||
cartesian_mtp_node.destroy_node()
|
||||
node.destroy_node()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -20,12 +20,14 @@ from rclpy.timer import Timer
|
|||
from ament_index_python.packages import get_package_share_directory
|
||||
from sensor_msgs.msg import Image
|
||||
from rbs_skill_interfaces.msg import BoundBox
|
||||
from rbs_skill_interfaces.srv import RbsBt
|
||||
|
||||
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
|
||||
|
||||
CONDITION_SRV_NAME = "/condition"
|
||||
FILE_DOPE_CONFIG = "od_yolo_config.yaml"
|
||||
CAMERA_LINK_DEFAULT = "rgbd_camera"
|
||||
NODE_NAME_DEFAULT = "lc_yolo" # the name doesn't matter in this node (defined in Launch)
|
||||
NODE_NAME_DEFAULT = "lc_yolo"
|
||||
PARAM_SKILL_CFG = "lc_yolo_cfg"
|
||||
|
||||
class ObjectDetection(Node):
|
||||
|
|
@ -60,7 +62,7 @@ class ObjectDetection(Node):
|
|||
str_cfg = self.get_parameter(PARAM_SKILL_CFG).get_parameter_value().string_value
|
||||
|
||||
self.skill_cfg = json.loads(str_cfg)
|
||||
self.nodeName = self.skill_cfg["Launch"]["name"]
|
||||
self.nodeName = NODE_NAME_DEFAULT # self.skill_cfg["Module"]["node_name"] #["Launch"]["name"]
|
||||
|
||||
self.camera_link = ""
|
||||
self.topicImage = ""
|
||||
|
|
@ -77,6 +79,17 @@ class ObjectDetection(Node):
|
|||
self.image_det = []
|
||||
self.bbox_res = None
|
||||
|
||||
self._srv_condition = self.create_service(RbsBt, NODE_NAME_DEFAULT + CONDITION_SRV_NAME, self.condition_callback)
|
||||
# , callback_group=self.cb_group)
|
||||
|
||||
def condition_callback(self, request, response):
|
||||
# is_success = self._interface(request.sid, request.action, request.command)
|
||||
_is = False
|
||||
if request.command == "isDetectionRun":
|
||||
_is = True
|
||||
response.ok = _is
|
||||
return response
|
||||
|
||||
def _set_camera_topic(self, camera_link: str):
|
||||
""" Service for camera name topics """
|
||||
self.camera_link = camera_link
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue