From 59c973bde6bc76f49cafb0f4f9ead645977f9127 Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Tue, 22 Nov 2022 10:28:27 +0900 Subject: [PATCH] Support ROS 2 Humble (#162) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * gazeboのコンフィグファイルをgazebo6のデフォルトファイルから移植 * gazebo動作確認のため一時的にコントローラyamlを移植 joint_state_broadcasterに変更 全ノードにuse_sim_timeを設定 * joint_state_broadcasterの更新 * controller_managerのoutputとspawnerを更新 * HardwareInterfaceのHumble対応 * CIのビルド環境修正 * .ci.rosinstallにgz_ros2_control追加 * ros_ign_gazeboをros_gz_simに更新 * joint_valuesの駆動速度を下げた * スタイル修正 * Update README * Update README * Update README * Update README.en.md Co-authored-by: Shota Aoki * Update README.md Co-authored-by: Shota Aoki * max_velocityを設定 * 不要になったファイルを削除 * 依存パッケージを更新 Co-authored-by: ShotaAk --- .ci.rosinstall | 4 + .github/workflows/industrial_ci.yml | 2 +- README.en.md | 26 ++-- README.md | 28 ++-- .../config/crane_x7_controllers.yaml | 2 +- .../crane_x7_control/crane_x7_hardware.hpp | 17 ++- .../launch/crane_x7_control.launch.py | 11 +- crane_x7_control/src/crane_x7_hardware.cpp | 53 ++++---- crane_x7_examples/README.md | 2 +- crane_x7_examples/src/joint_values.cpp | 4 +- crane_x7_gazebo/gui/gui.config | 128 ++++++++++++++++-- .../launch/crane_x7_with_table.launch.py | 10 +- crane_x7_gazebo/package.xml | 2 +- .../config/joint_limits.yaml | 9 ++ .../launch/run_move_group.launch.py | 4 +- 15 files changed, 215 insertions(+), 87 deletions(-) diff --git a/.ci.rosinstall b/.ci.rosinstall index 56ef2880..a4dc1965 100644 --- a/.ci.rosinstall +++ b/.ci.rosinstall @@ -2,3 +2,7 @@ uri: https://github.com/rt-net/crane_x7_description.git local-name: crane_x7_description version: ros2 +- git: + uri: https://github.com/ros-controls/gz_ros2_control.git + local-name: gz_ros2_control + version: master diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index e949e626..95c0d087 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -17,7 +17,7 @@ jobs: strategy: matrix: env: - - { ROS_DISTRO: foxy, ROS_REPO: ros } + - { ROS_DISTRO: humble, ROS_REPO: ros } runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 diff --git a/README.en.md b/README.en.md index 14f60727..fbe2f4a0 100644 --- a/README.en.md +++ b/README.en.md @@ -10,18 +10,21 @@ ROS 2 package suite of CRANE-X7. ## Table of Contents -- [Supported ROS 2 distributions](#supported-ros-2-distributions) - - [ROS](#ros) -- [Requirements](#requirements) -- [Installation](#installation) - - [Build from source](#build-from-source) -- [Quick Start](#quick-start) -- [Packages](#packages) -- [License](#license) +- [crane_x7_ros](#crane_x7_ros) + - [Table of Contents](#table-of-contents) + - [Supported ROS 2 distributions](#supported-ros-2-distributions) + - [ROS](#ros) + - [Requirements](#requirements) + - [Installation](#installation) + - [Build from source](#build-from-source) + - [Quick Start](#quick-start) + - [Packages](#packages) + - [License](#license) ## Supported ROS 2 distributions -- Foxy +- [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel) +- Humble ### ROS @@ -35,7 +38,7 @@ ROS 2 package suite of CRANE-X7. - Linux OS - Ubuntu 20.04 - ROS - - [Foxy Fitzroy](https://docs.ros.org/en/foxy/Installation.html) + - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) ## Installation @@ -43,7 +46,7 @@ ROS 2 package suite of CRANE-X7. ```sh # Setup ROS environment -$ source /opt/ros/foxy/setup.bash +$ source /opt/ros/humble/setup.bash # Download crane_x7 repositories $ mkdir -p ~/ros2_ws/src @@ -52,6 +55,7 @@ $ git clone -b ros2 https://github.com/rt-net/crane_x7_ros.git $ git clone -b ros2 https://github.com/rt-net/crane_x7_description.git # Install dependencies +$ git clone https://github.com/ros-controls/gz_ros2_control.git $ rosdep install -r -y -i --from-paths . # Build & Install diff --git a/README.md b/README.md index 0ce783ff..07c62ca7 100644 --- a/README.md +++ b/README.md @@ -10,19 +10,22 @@ ROS 2 package suite of CRANE-X7. ## Table of Contents -- [Supported ROS 2 distributions](#supported-ros-2-distributions) - - [ROS](#ros) -- [Requirements](#requirements) -- [Installation](#installation) - - [Build from source](#build-from-source) -- [Quick Start](#quick-start) -- [Packages](#packages) -- [ライセンス](#ライセンス) -- [開発について](#開発について) +- [crane_x7_ros](#crane_x7_ros) + - [Table of Contents](#table-of-contents) + - [Supported ROS 2 distributions](#supported-ros-2-distributions) + - [ROS](#ros) + - [Requirements](#requirements) + - [Installation](#installation) + - [Build from source](#build-from-source) + - [Quick Start](#quick-start) + - [Packages](#packages) + - [ライセンス](#ライセンス) + - [開発について](#開発について) ## Supported ROS 2 distributions -- Foxy +- [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel) +- Humble ### ROS @@ -37,7 +40,7 @@ ROS 2 package suite of CRANE-X7. - Linux OS - Ubuntu 20.04 - ROS - - [Foxy Fitzroy](https://docs.ros.org/en/foxy/Installation.html) + - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) ## Installation @@ -45,7 +48,7 @@ ROS 2 package suite of CRANE-X7. ```sh # Setup ROS environment -$ source /opt/ros/foxy/setup.bash +$ source /opt/ros/humble/setup.bash # Download crane_x7 repositories $ mkdir -p ~/ros2_ws/src @@ -54,6 +57,7 @@ $ git clone -b ros2 https://github.com/rt-net/crane_x7_ros.git $ git clone -b ros2 https://github.com/rt-net/crane_x7_description.git # Install dependencies +$ git clone https://github.com/ros-controls/gz_ros2_control.git $ rosdep install -r -y -i --from-paths . # Build & Install diff --git a/crane_x7_control/config/crane_x7_controllers.yaml b/crane_x7_control/config/crane_x7_controllers.yaml index ee64934e..43e74bd2 100644 --- a/crane_x7_control/config/crane_x7_controllers.yaml +++ b/crane_x7_control/config/crane_x7_controllers.yaml @@ -7,7 +7,7 @@ controller_manager: crane_x7_gripper_controller: type: position_controllers/GripperActionController joint_state_controller: - type: joint_state_controller/JointStateController + type: joint_state_broadcaster/JointStateBroadcaster crane_x7_arm_controller: ros__parameters: diff --git a/crane_x7_control/include/crane_x7_control/crane_x7_hardware.hpp b/crane_x7_control/include/crane_x7_control/crane_x7_hardware.hpp index 3ff745c5..5882c388 100644 --- a/crane_x7_control/include/crane_x7_control/crane_x7_hardware.hpp +++ b/crane_x7_control/include/crane_x7_control/crane_x7_hardware.hpp @@ -20,18 +20,21 @@ #include #include "crane_x7_control/visibility_control.h" -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/rclcpp.hpp" #include "rt_manipulators_cpp/hardware.hpp" +#include "rclcpp_lifecycle/state.hpp" using hardware_interface::return_type; +using hardware_interface::CallbackReturn; namespace crane_x7_control { class CraneX7Hardware : public - hardware_interface::BaseInterface + hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(CraneX7Hardware) @@ -40,7 +43,7 @@ class CraneX7Hardware : public ~CraneX7Hardware(); CRANE_X7_CONTROL_PUBLIC - return_type configure(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; CRANE_X7_CONTROL_PUBLIC std::vector export_state_interfaces() override; @@ -49,16 +52,16 @@ class CraneX7Hardware : public std::vector export_command_interfaces() override; CRANE_X7_CONTROL_PUBLIC - return_type start() override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; CRANE_X7_CONTROL_PUBLIC - return_type stop() override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; CRANE_X7_CONTROL_PUBLIC - return_type read() override; + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; CRANE_X7_CONTROL_PUBLIC - return_type write() override; + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; private: bool communication_timeout(); diff --git a/crane_x7_control/launch/crane_x7_control.launch.py b/crane_x7_control/launch/crane_x7_control.launch.py index 3ac0e036..beec5009 100644 --- a/crane_x7_control/launch/crane_x7_control.launch.py +++ b/crane_x7_control/launch/crane_x7_control.launch.py @@ -61,26 +61,23 @@ def generate_launch_description(): executable='ros2_control_node', parameters=[{'robot_description': LaunchConfiguration('loaded_description')}, crane_x7_controllers], - output={ - 'stdout': 'screen', - 'stderr': 'screen', - }, + output='screen', ) spawn_joint_state_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner.py joint_state_controller'], + cmd=['ros2 run controller_manager spawner joint_state_controller'], shell=True, output='screen', ) spawn_arm_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner.py crane_x7_arm_controller'], + cmd=['ros2 run controller_manager spawner crane_x7_arm_controller'], shell=True, output='screen', ) spawn_gripper_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner.py crane_x7_gripper_controller'], + cmd=['ros2 run controller_manager spawner crane_x7_gripper_controller'], shell=True, output='screen', ) diff --git a/crane_x7_control/src/crane_x7_hardware.cpp b/crane_x7_control/src/crane_x7_hardware.cpp index 78332aa9..8be96949 100644 --- a/crane_x7_control/src/crane_x7_hardware.cpp +++ b/crane_x7_control/src/crane_x7_hardware.cpp @@ -46,11 +46,11 @@ CraneX7Hardware::~CraneX7Hardware() } } -return_type CraneX7Hardware::configure( +CallbackReturn CraneX7Hardware::on_init( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != return_type::OK) { - return return_type::ERROR; + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; } hw_position_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -66,7 +66,7 @@ return_type CraneX7Hardware::configure( RCLCPP_ERROR( LOGGER, "Joint '%s' does not have 'current_to_effort' parameter.", joint.name.c_str()); - return return_type::ERROR; + return CallbackReturn::ERROR; } } @@ -74,9 +74,9 @@ return_type CraneX7Hardware::configure( if (joint.command_interfaces.size() != 1) { RCLCPP_FATAL( LOGGER, - "Joint '%s' has %d command interfaces. 1 expected.", + "Joint '%s' has %ld command interfaces. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return return_type::ERROR; + return CallbackReturn::ERROR; } if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION)) { @@ -85,15 +85,15 @@ return_type CraneX7Hardware::configure( "Joint '%s' has %s command interface. Expected %s, %s, or %s.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); - return return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces.size() < 1) { RCLCPP_FATAL( LOGGER, - "Joint '%s'has %d state interfaces. At least 1 expected.", + "Joint '%s'has %ld state interfaces. At least 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); - return return_type::ERROR; + return CallbackReturn::ERROR; } for (auto state_interface : joint.state_interfaces) { @@ -106,7 +106,7 @@ return_type CraneX7Hardware::configure( "Joint '%s' has %s state interface. Expected %s, %s, or %s.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_EFFORT); - return return_type::ERROR; + return CallbackReturn::ERROR; } } } @@ -119,19 +119,18 @@ return_type CraneX7Hardware::configure( if (!hardware_->connect(baudrate)) { RCLCPP_ERROR(LOGGER, "Failed to connect a robot."); - return return_type::ERROR; + return CallbackReturn::ERROR; } if (!hardware_->load_config_file(config_file_path)) { RCLCPP_ERROR(LOGGER, "Failed to read a config file."); - return return_type::ERROR; + return CallbackReturn::ERROR; } timeout_seconds_ = std::stod(info_.hardware_parameters["timeout_seconds"]); steady_clock_ = rclcpp::Clock(RCL_STEADY_TIME); - status_ = hardware_interface::status::CONFIGURED; - return return_type::OK; + return CallbackReturn::SUCCESS; } std::vector @@ -168,14 +167,14 @@ CraneX7Hardware::export_command_interfaces() return command_interfaces; } -return_type CraneX7Hardware::start() +CallbackReturn CraneX7Hardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { // Set current timestamp to disable the communication timeout. prev_comm_timestamp_ = steady_clock_.now(); timeout_has_printed_ = false; // Set present joint positions to hw_position_commands for safe start-up. - read(); + read(prev_comm_timestamp_, rclcpp::Duration::from_seconds(0)); for (std::size_t i = 0; i < hw_position_commands_.size(); i++) { double present_position = hw_position_states_[i]; double limit_min = present_position; @@ -188,39 +187,38 @@ return_type CraneX7Hardware::start() } hw_position_commands_[i] = std::clamp(present_position, limit_min, limit_max); } - write(); + write(prev_comm_timestamp_, rclcpp::Duration::from_seconds(0)); if (!hardware_->write_position_pid_gain_to_group( GROUP_NAME, START_P_GAIN, START_I_GAIN, START_D_GAIN)) { RCLCPP_ERROR(LOGGER, "Failed to set PID gains."); - return return_type::ERROR; + return CallbackReturn::ERROR; } if (!hardware_->torque_on(GROUP_NAME)) { RCLCPP_ERROR(LOGGER, "Failed to set torque on."); - return return_type::ERROR; + return CallbackReturn::ERROR; } - status_ = hardware_interface::status::STARTED; - return return_type::OK; + return CallbackReturn::SUCCESS; } -return_type CraneX7Hardware::stop() +CallbackReturn CraneX7Hardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { // Set low PID gains for safe stopping. if (!hardware_->write_position_pid_gain_to_group( GROUP_NAME, STOP_P_GAIN, STOP_I_GAIN, STOP_D_GAIN)) { RCLCPP_ERROR(LOGGER, "Failed to set PID gains."); - return return_type::ERROR; + return CallbackReturn::ERROR; } - status_ = hardware_interface::status::STOPPED; - return return_type::OK; + return CallbackReturn::SUCCESS; } -return_type CraneX7Hardware::read() +return_type CraneX7Hardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { if (communication_timeout()) { if (!timeout_has_printed_) { @@ -269,7 +267,8 @@ return_type CraneX7Hardware::read() return return_type::OK; } -return_type CraneX7Hardware::write() +return_type CraneX7Hardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { if (communication_timeout()) { if (!timeout_has_printed_) { diff --git a/crane_x7_examples/README.md b/crane_x7_examples/README.md index 2fe5a84a..fe1c9783 100644 --- a/crane_x7_examples/README.md +++ b/crane_x7_examples/README.md @@ -155,7 +155,7 @@ ros2 launch crane_x7_examples example.launch.py example:='joint_values' ### cartesian_path -[Cartesian Path](https://moveit.picknik.ai/foxy/doc/move_group_interface/move_group_interface_tutorial.html#cartesian-paths) +[Cartesian Path](https://moveit.picknik.ai/humble/doc/examples/move_group_interface/move_group_interface_tutorial.html#cartesian-paths) を生成し、手先で円を描くコード例です。 次のコマンドを実行します。 diff --git a/crane_x7_examples/src/joint_values.cpp b/crane_x7_examples/src/joint_values.cpp index 2592323d..f2575360 100644 --- a/crane_x7_examples/src/joint_values.cpp +++ b/crane_x7_examples/src/joint_values.cpp @@ -38,8 +38,8 @@ int main(int argc, char ** argv) MoveGroupInterface move_group_arm(move_group_node, "arm"); // 駆動速度を調整する - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 - move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(0.5); // Set 0.0 ~ 1.0 + move_group_arm.setMaxAccelerationScalingFactor(0.5); // Set 0.0 ~ 1.0 // SRDFに定義されている"vertical"の姿勢にする // すべてのジョイントの目標角度が0度になる diff --git a/crane_x7_gazebo/gui/gui.config b/crane_x7_gazebo/gui/gui.config index ab1db710..0d9558d0 100644 --- a/crane_x7_gazebo/gui/gui.config +++ b/crane_x7_gazebo/gui/gui.config @@ -27,7 +27,7 @@ - + 3D View false @@ -41,7 +41,82 @@ 1.0 0 2.0 0 0.5 3.14 - + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + World control @@ -61,10 +136,11 @@ true true true + true - + World stats @@ -85,11 +161,10 @@ true true true - - - + + false 0 @@ -102,8 +177,8 @@ - - + + false 250 @@ -116,17 +191,48 @@ + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + false + + false - 400 - 0 + 250 + 50 50 50 floating false - #666666 + #777777 + + + + + + + false + 300 + 50 + 100 + 50 + floating + false + #777777 diff --git a/crane_x7_gazebo/launch/crane_x7_with_table.launch.py b/crane_x7_gazebo/launch/crane_x7_with_table.launch.py index 73cafa69..59801bf5 100644 --- a/crane_x7_gazebo/launch/crane_x7_with_table.launch.py +++ b/crane_x7_gazebo/launch/crane_x7_with_table.launch.py @@ -21,6 +21,7 @@ from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node +from launch_ros.actions import SetParameter def generate_launch_description(): @@ -41,7 +42,7 @@ def generate_launch_description(): ) ignition_spawn_entity = Node( - package='ros_ign_gazebo', + package='ros_gz_sim', executable='create', output='screen', arguments=['-topic', '/robot_description', @@ -64,24 +65,25 @@ def generate_launch_description(): ) spawn_joint_state_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner.py joint_state_controller'], + cmd=['ros2 run controller_manager spawner joint_state_controller'], shell=True, output='screen', ) spawn_arm_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner.py crane_x7_arm_controller'], + cmd=['ros2 run controller_manager spawner crane_x7_arm_controller'], shell=True, output='screen', ) spawn_gripper_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner.py crane_x7_gripper_controller'], + cmd=['ros2 run controller_manager spawner crane_x7_gripper_controller'], shell=True, output='screen', ) return LaunchDescription([ + SetParameter(name='use_sim_time', value=True), ign_gazebo, ignition_spawn_entity, move_group, diff --git a/crane_x7_gazebo/package.xml b/crane_x7_gazebo/package.xml index 44efcecf..601de2bf 100644 --- a/crane_x7_gazebo/package.xml +++ b/crane_x7_gazebo/package.xml @@ -18,7 +18,7 @@ controller_manager gripper_controllers ros2_controllers - ros_ign + ros_gz robot_state_publisher diff --git a/crane_x7_moveit_config/config/joint_limits.yaml b/crane_x7_moveit_config/config/joint_limits.yaml index 61e6e488..41b10fbe 100644 --- a/crane_x7_moveit_config/config/joint_limits.yaml +++ b/crane_x7_moveit_config/config/joint_limits.yaml @@ -9,36 +9,45 @@ joint_limits: crane_x7_gripper_finger_a_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 crane_x7_gripper_finger_b_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 crane_x7_lower_arm_fixed_part_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 crane_x7_lower_arm_revolute_part_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 crane_x7_shoulder_fixed_part_pan_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 crane_x7_shoulder_revolute_part_tilt_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 crane_x7_upper_arm_revolute_part_rotate_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 crane_x7_upper_arm_revolute_part_twist_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 crane_x7_wrist_joint: has_velocity_limits: true has_acceleration_limits: true + max_velocity: 4.8 max_acceleration: 4.4 \ No newline at end of file diff --git a/crane_x7_moveit_config/launch/run_move_group.launch.py b/crane_x7_moveit_config/launch/run_move_group.launch.py index db7b7d0a..365e2bc4 100644 --- a/crane_x7_moveit_config/launch/run_move_group.launch.py +++ b/crane_x7_moveit_config/launch/run_move_group.launch.py @@ -22,8 +22,8 @@ from launch_ros.actions import Node import yaml -# Reference: https://github.com/ros-planning/moveit2/blob/foxy/moveit_demo_nodes/ -# run_move_group/launch/run_move_group.launch.py +# Reference: https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ +# examples/move_group_interface/launch/move_group.launch.py def load_file(package_name, file_path):