From 6ad8e387a31a73269f15669a16b7bf9bdfb021da Mon Sep 17 00:00:00 2001 From: shalenikol Date: Tue, 17 Sep 2024 08:48:20 +0000 Subject: [PATCH] ADD: Node for recording demo in BT --- rbs_utils/rbs_utils/CMakeLists.txt | 4 + rbs_utils/rbs_utils/package.xml | 2 + rbs_utils/rbs_utils/scripts/recording_demo.md | 89 ++++++++++ .../scripts/recording_demo_via_rosbag.py | 160 ++++++++++++++++++ 4 files changed, 255 insertions(+) create mode 100644 rbs_utils/rbs_utils/scripts/recording_demo.md create mode 100755 rbs_utils/rbs_utils/scripts/recording_demo_via_rosbag.py diff --git a/rbs_utils/rbs_utils/CMakeLists.txt b/rbs_utils/rbs_utils/CMakeLists.txt index 438774a..9bbfa0c 100644 --- a/rbs_utils/rbs_utils/CMakeLists.txt +++ b/rbs_utils/rbs_utils/CMakeLists.txt @@ -13,9 +13,11 @@ find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) find_package(rviz_visual_tools REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(rbs_utils_interfaces REQUIRED) find_package(dynmsg REQUIRED) +find_package(rclpy REQUIRED) set(deps rclcpp @@ -24,6 +26,7 @@ set(deps tf2_eigen rviz_visual_tools geometry_msgs + sensor_msgs rbs_utils_interfaces dynmsg ) @@ -34,6 +37,7 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS scripts/assembly_config_service.py + scripts/recording_demo_via_rosbag.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/rbs_utils/rbs_utils/package.xml b/rbs_utils/rbs_utils/package.xml index d730d91..5609d7f 100644 --- a/rbs_utils/rbs_utils/package.xml +++ b/rbs_utils/rbs_utils/package.xml @@ -9,6 +9,7 @@ ament_cmake + rclpy rclcpp rclcpp_lifecycle tf2_ros @@ -19,6 +20,7 @@ std_msgs rbs_utils_interfaces dynmsg + sensor_msgs ament_lint_auto ament_lint_common diff --git a/rbs_utils/rbs_utils/scripts/recording_demo.md b/rbs_utils/rbs_utils/scripts/recording_demo.md new file mode 100644 index 0000000..b98026d --- /dev/null +++ b/rbs_utils/rbs_utils/scripts/recording_demo.md @@ -0,0 +1,89 @@ +### Пример запуска записи демонстрации в RosBag2 + +Подготовить папку с файлами BT v.4 +* Папка /path/to/bt/ +* bt.xml +```xml + + + + + + + + + + + + + + + + + +``` +* skills.json +```json +{ + "skills": [ + { + "sid": "a", + "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, + "Module": { "name": "RecordingDemo", "description": "Recording a demo via RosBag", "node_name": "lc_record_demo" }, + "Launch": { "package": "rbs_utils", "executable": "recording_demo_via_rosbag.py" }, + "BTAction": [ + { + "name": "rdConfigure", + "type_action": "action", + "type": "run", + "param": [], + "result":[] + }, + { "name": "rdStop", "type_action": "action", "type": "stop", "param": [], "result": [] } + ], + "Settings": [ + { "name": "output_path", "value": "rbs_testbag", "description": "!!! Путь, куда запишется rosbag. Если он уже есть - перетрётся!" } + ] + }, + { + "sid": "c", + "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, + "Module": { "name": "MoveToPose", "description": "Move to Pose skill with cartesian controllers", "node_name": "mtp_cartesian" }, + "Launch": { "package": "rbss_movetopose", "executable": "mtp_cartesian.py" }, + "BTAction": [ + { + "name": "move", + "type_action": "action", + "type": "action", + "param": [ + { + "type": "move_to_pose", + "dependency": { "robot_name": "arm0", + "pose": { "position": {"x":0.1, "y":0.1, "z":0.9}, "orientation": {"x":0.55, "y":0.0, "z":0.45, "w": 1.0} } } + } + ] + } + ], + "Settings": [ + { "name": "server_name", "value": "cartesian_move_to_pose" }, + { "name": "end_effector_velocity", "value": 1.0 }, + { "name": "end_effector_acceleration", "value": 1.0 } + ] + } + ] +} +``` + +Запустить последовательно три терминала и выполнить запуск 3-х launch-файлов: +* 1 - запуск интерфейсной ноды с серверами навыков +```bash +ros2 launch rbs_interface interface.launch.py bt_path:=/path/to/bt +``` +* 2 - запуск робота в сцене +```bash +ros2 launch rbs_bringup single_robot.launch.py +``` +* 3 - запуск дерева поведения +```bash +ros2 launch rbs_bt_executor rbs_bt_web.launch.py bt_path:=/path/to/bt +``` diff --git a/rbs_utils/rbs_utils/scripts/recording_demo_via_rosbag.py b/rbs_utils/rbs_utils/scripts/recording_demo_via_rosbag.py new file mode 100755 index 0000000..663dbac --- /dev/null +++ b/rbs_utils/rbs_utils/scripts/recording_demo_via_rosbag.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +""" + recording_demo_lifecycle_node_via_RosBag + ROS 2 program for recording a demo via RosBag + + @shalenikol release 0.1 +""" +import os +import shutil +import json +from functools import partial + +import rclpy +from rclpy.lifecycle import Node +from rclpy.lifecycle import State +from rclpy.lifecycle import TransitionCallbackReturn + +import rosbag2_py +from ros2topic.api import get_topic_names_and_types +from rclpy.serialization import serialize_message + +from sensor_msgs.msg import Image, JointState + +NODE_NAME_DEFAULT = "lc_record_demo" +PARAM_SKILL_CFG = "lc_record_demo_cfg" + +OUTPUT_PATH_DEFAULT = "my_bag" +WRITER_ID = "sqlite3" +TOPIC_TYPES = ["sensor_msgs/msg/JointState", "sensor_msgs/msg/Image"] +TOPIC_CLASSES = [JointState, Image] + +class RecordingDemo(Node): + """ lifecycle node """ + + def __init__(self, **kwargs): + """Construct the node.""" + self.mode: int = 0 # 0 - нет записи, 1 - есть запись + self.topics = [] + self._sub = [] + self.output_path = OUTPUT_PATH_DEFAULT + + # 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) + sets = self.skill_cfg["Settings"] + for s in sets: + if s["name"] == "output_path": + self.output_path = s["value"] + + if os.path.isdir(self.output_path): + shutil.rmtree(self.output_path) + + self.writer = rosbag2_py.SequentialWriter() + storage_options = rosbag2_py._storage.StorageOptions( + uri=self.output_path, + storage_id=WRITER_ID) + converter_options = rosbag2_py._storage.ConverterOptions("", "") + self.writer.open(storage_options, converter_options) + + def get_list_topics(self) -> list: + topic_names_and_types = get_topic_names_and_types(node=self, include_hidden_topics=False) + + topic_list = [] + for (topic_name, topic_types) in topic_names_and_types: + if topic_types[0] in TOPIC_TYPES: + i = TOPIC_TYPES.index(topic_types[0]) + topic_list.append({"name": topic_name, "type": topic_types[0], "class": TOPIC_CLASSES[i]}) + + return topic_list + + def on_configure(self, state: State) -> TransitionCallbackReturn: + """ + Configure the node, after a configuring transition is requested. + + :return: The state machine either invokes a transition to the "inactive" state or stays + in "unconfigured" depending on the return value. + TransitionCallbackReturn.SUCCESS transitions to "inactive". + TransitionCallbackReturn.FAILURE transitions to "unconfigured". + TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing" + """ + + self.topics = self.get_list_topics() + for id, topic in enumerate(self.topics): + topic_info = rosbag2_py._storage.TopicMetadata( + name=topic["name"], + type=topic["type"], + serialization_format="cdr") + self.writer.create_topic(topic_info) + self.get_logger().info(f"on_configure: {id}) {topic}") + + # self.get_logger().info("on_configure() is called.") + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info('on_activate() is called.') + # Create the subscribers. + for id, topic in enumerate(self.topics): + sub = self.create_subscription(topic["class"], topic["name"], + partial(self.topic_callback, topic["name"]), + 10) + self._sub.append(sub) + + self.mode += 1 + return super().on_activate(state) + + def on_deactivate(self, state: State) -> TransitionCallbackReturn: + self.get_logger().info('on_deactivate() is called.') + # Destroy the subscribers. + for sub in self._sub: + self.destroy_subscription(sub) + self._sub.clear() + + self.mode -= 1 + return super().on_deactivate(state) + + def on_cleanup(self, state: State) -> TransitionCallbackReturn: + """ + Cleanup the node. + """ + for sub in self._sub: + self.destroy_subscription(sub) + self._sub.clear() + + self.get_logger().info('on_cleanup() is called.') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: State) -> TransitionCallbackReturn: + """ + Shutdown the node. + """ + for sub in self._sub: + self.destroy_subscription(sub) + self._sub.clear() + + self.get_logger().info('on_shutdown() is called.') + return TransitionCallbackReturn.SUCCESS + + def topic_callback(self, topic_name, msg): + self.writer.write( + topic_name, + serialize_message(msg), + self.get_clock().now().nanoseconds) + +def main(): + rclpy.init() + + executor = rclpy.executors.SingleThreadedExecutor() + lc_node = RecordingDemo() + executor.add_node(lc_node) + try: + executor.spin() + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + lc_node.destroy_node() + +if __name__ == '__main__': + main()