diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt index 7037bec..6600143 100644 --- a/rbs_bt_executor/CMakeLists.txt +++ b/rbs_bt_executor/CMakeLists.txt @@ -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}) diff --git a/rbs_bt_executor/launch/rbs_bt_web.launch.py b/rbs_bt_executor/launch/rbs_bt_web.launch.py index a6d05ba..cd16d3b 100644 --- a/rbs_bt_executor/launch/rbs_bt_web.launch.py +++ b/rbs_bt_executor/launch/rbs_bt_web.launch.py @@ -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}] + 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 diff --git a/rbs_bt_executor/src/rbsBTAction.cpp b/rbs_bt_executor/src/rbsBTAction.cpp index e28bd09..0675785 100644 --- a/rbs_bt_executor/src/rbsBTAction.cpp +++ b/rbs_bt_executor/src/rbsBTAction.cpp @@ -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 m_params_client; }; -CreateRosNodePlugin(RbsBtAction, "RbsAction") +// !!! теперь устаревшая версия !!! +CreateRosNodePlugin(RbsBtAction, "RbsBtAction") diff --git a/rbs_interface/CMakeLists.txt b/rbs_interface/CMakeLists.txt index 2b62a1f..7a5ba4b 100644 --- a/rbs_interface/CMakeLists.txt +++ b/rbs_interface/CMakeLists.txt @@ -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} diff --git a/rbs_interface/launch/interface.launch.py b/rbs_interface/launch/interface.launch.py index 8a2fb44..ae66c0f 100644 --- a/rbs_interface/launch/interface.launch.py +++ b/rbs_interface/launch/interface.launch.py @@ -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 diff --git a/rbs_interface/package.xml b/rbs_interface/package.xml index 3591e93..ad45f46 100644 --- a/rbs_interface/package.xml +++ b/rbs_interface/package.xml @@ -17,7 +17,8 @@ sensor_msgs std_msgs rbs_skill_interfaces - + behaviortree_ros2 + ament_lint_auto ament_lint_common diff --git a/rbs_interface/scripts/rbs_interface.py b/rbs_interface/scripts/rbs_interface.py index 38b29d4..31735f3 100755 --- a/rbs_interface/scripts/rbs_interface.py +++ b/rbs_interface/scripts/rbs_interface.py @@ -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}') + # 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): @@ -85,8 +94,8 @@ class rbsInterface(Node): future = self.cli.call_async(req) self.executor.spin_until_future_complete(future) - res = future.result() - + 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(): diff --git a/rbs_interface/src/BTExec.cpp b/rbs_interface/src/BTExec.cpp new file mode 100644 index 0000000..5f7aba1 --- /dev/null +++ b/rbs_interface/src/BTExec.cpp @@ -0,0 +1,201 @@ +#include +#include +#include +#include +#include +#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 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 { +public: + RbsAction(const std::string &name, const NodeConfig &conf, const RosNodeParams ¶ms) + : RosActionNode(name, conf, params) { + + m_action = getInput(STR_ACTION).value(); + m_command = getInput(STR_COMMAND).value(); + RCLCPP_INFO_STREAM(logger(), "Start node RbsAction: " + m_action + "/" + m_command); + + m_client = std::make_shared(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(STR_SID), + BT::InputPort(STR_ACTION), + BT::InputPort(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 m_client; +}; + +// ********************************************************** +class RbsCondition : public RosServiceNode { +public: + RbsCondition(const std::string &name, const NodeConfig& conf, const RosNodeParams& params) + : RosServiceNode(name, conf, params) { + + m_action_name = getInput(STR_ACTION).value(); + m_command = getInput(STR_COMMAND).value(); + RCLCPP_INFO_STREAM(logger(), "Start node RbsCondition: " + m_action_name + "/" + m_command); + + m_params_client = std::make_shared(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(STR_SID), + InputPort(STR_ACTION), + InputPort(STR_COMMAND) + }); + } + +private: + int m_i{0}; + std::string m_action_name{}; + std::string m_command{}; + std::shared_ptr m_params_client; +}; + +// class RbsCondition : public RosTopicSubNode { +// } +// Simple function that return a NodeStatus +// BT::NodeStatus TestIf() +// { +// std::string m_if = "if"; //getInput("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(fh), std::istreambuf_iterator()}; + + rclcpp::init(argc, argv); + + // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), xml.c_str()); + auto nh = std::make_shared("bt_exec"); //"_action"); + // auto nh2 = std::make_shared("bt_exe_condition"); + + RosNodeParams params; //,params2; + params.nh = nh; + params.default_port_value = SERVICE_NAME; + params.server_timeout = 3s; + + BehaviorTreeFactory factory; + factory.registerNodeType("Is", params); + + // params.nh = nh; + params.default_port_value = SERVER_NAME; + // params.server_timeout = 3s; + factory.registerNodeType("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; +} diff --git a/rbs_skill_interfaces/CMakeLists.txt b/rbs_skill_interfaces/CMakeLists.txt index 7eb04a8..1dc670a 100644 --- a/rbs_skill_interfaces/CMakeLists.txt +++ b/rbs_skill_interfaces/CMakeLists.txt @@ -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" diff --git a/rbs_skill_interfaces/action/RbsBt.action b/rbs_skill_interfaces/action/RbsBt.action new file mode 100644 index 0000000..00f60ce --- /dev/null +++ b/rbs_skill_interfaces/action/RbsBt.action @@ -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 diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index fe5f48b..e3b9135 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/rbs_skill_servers/launch/skills.launch.py @@ -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 diff --git a/rbss_movetopose/CMakeLists.txt b/rbss_movetopose/CMakeLists.txt new file mode 100644 index 0000000..9b9c366 --- /dev/null +++ b/rbss_movetopose/CMakeLists.txt @@ -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() diff --git a/rbss_movetopose/launch/movetopose.launch.py b/rbss_movetopose/launch/movetopose.launch.py new file mode 100644 index 0000000..be6230e --- /dev/null +++ b/rbss_movetopose/launch/movetopose.launch.py @@ -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) diff --git a/rbss_movetopose/package.xml b/rbss_movetopose/package.xml new file mode 100644 index 0000000..2ddc61f --- /dev/null +++ b/rbss_movetopose/package.xml @@ -0,0 +1,24 @@ + + + + rbss_movetopose + 0.0.1 + The Move to pose skill + shalenikol + Apache License 2.0 + + ament_cmake + ament_cmake_python + + rclpy + rclcpp + std_msgs + rbs_skill_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rbss_movetopose/rbs_package.json b/rbss_movetopose/rbs_package.json new file mode 100644 index 0000000..8d72a60 --- /dev/null +++ b/rbss_movetopose/rbs_package.json @@ -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 } + ] +} diff --git a/rbss_movetopose/rbss_movetopose/__init__.py b/rbss_movetopose/rbss_movetopose/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rbss_movetopose/scripts/mtp_cartesian.py b/rbss_movetopose/scripts/mtp_cartesian.py new file mode 100755 index 0000000..5fd5405 --- /dev/null +++ b/rbss_movetopose/scripts/mtp_cartesian.py @@ -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): + """ 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): + """ 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() diff --git a/rbss_objectdetection/scripts/od_yolo_lc.py b/rbss_objectdetection/scripts/od_yolo_lc.py index cb32b9c..e71d445 100755 --- a/rbss_objectdetection/scripts/od_yolo_lc.py +++ b/rbss_objectdetection/scripts/od_yolo_lc.py @@ -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