From 3648242f50e6f15636df7b074284637723a0c4dc Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 14 Aug 2024 18:13:57 +0900 Subject: [PATCH] =?UTF-8?q?=E7=99=BB=E9=8C=B2=E4=BD=8D=E7=BD=AE=E3=81=B8?= =?UTF-8?q?=E3=81=AE=E7=A7=BB=E5=8B=95=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/example.launch.py | 93 +++++++++++++++++++ sciurus17_examples_py/package.xml | 23 +++++ .../resource/sciurus17_examples_py | 0 .../sciurus17_examples_py/__init__.py | 0 .../pick_and_place_left_arm.py | 70 ++++++++++++++ sciurus17_examples_py/setup.cfg | 4 + sciurus17_examples_py/setup.py | 30 ++++++ sciurus17_examples_py/test/test_copyright.py | 25 +++++ sciurus17_examples_py/test/test_flake8.py | 25 +++++ sciurus17_examples_py/test/test_pep257.py | 23 +++++ .../config/moveit_cpp.yaml | 26 ++++++ 11 files changed, 319 insertions(+) create mode 100644 sciurus17_examples_py/launch/example.launch.py create mode 100644 sciurus17_examples_py/package.xml create mode 100644 sciurus17_examples_py/resource/sciurus17_examples_py create mode 100644 sciurus17_examples_py/sciurus17_examples_py/__init__.py create mode 100755 sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py create mode 100644 sciurus17_examples_py/setup.cfg create mode 100644 sciurus17_examples_py/setup.py create mode 100644 sciurus17_examples_py/test/test_copyright.py create mode 100644 sciurus17_examples_py/test/test_flake8.py create mode 100644 sciurus17_examples_py/test/test_pep257.py create mode 100644 sciurus17_moveit_config/config/moveit_cpp.yaml diff --git a/sciurus17_examples_py/launch/example.launch.py b/sciurus17_examples_py/launch/example.launch.py new file mode 100644 index 00000000..d55d5e12 --- /dev/null +++ b/sciurus17_examples_py/launch/example.launch.py @@ -0,0 +1,93 @@ +# Copyright 2023 RT Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from sciurus17_description.robot_description_loader import RobotDescriptionLoader +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import SetParameter +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +import yaml +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch +from moveit_configs_utils.launches import generate_moveit_rviz_launch +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch +from moveit_configs_utils.launches import generate_rsp_launch +from sciurus17_description.robot_description_loader import RobotDescriptionLoader +from ament_index_python.packages import get_package_share_directory + + +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + ld = LaunchDescription() + description_loader = RobotDescriptionLoader() + + ld.add_action( + DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in sciurus17_description.' + ) + ) + + moveit_config = ( + MoveItConfigsBuilder("sciurus17") + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .trajectory_execution(file_path="config/moveit_controllers.yaml") + .planning_pipelines(pipelines=["ompl"]) + .moveit_cpp( + file_path=get_package_share_directory("sciurus17_moveit_config") + + "/config/moveit_cpp.yaml" + ) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } + + moveit_config.move_group_capabilities = { + "capabilities": "" + } + + declare_example_name = DeclareLaunchArgument( + 'example', default_value='pick_and_place_left_arm', + description=('Set an example executable name: ' + '[gripper_control, neck_control, waist_control,' + 'pick_and_place_right_arm_waist, pick_and_place_left_arm]') + ) + + example_node = Node( + name=[LaunchConfiguration('example'), '_node'], + package='sciurus17_examples_py', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[moveit_config.to_dict()], + ) + + ld.add_action(declare_example_name) + ld.add_action(example_node) + + return ld diff --git a/sciurus17_examples_py/package.xml b/sciurus17_examples_py/package.xml new file mode 100644 index 00000000..aecc1425 --- /dev/null +++ b/sciurus17_examples_py/package.xml @@ -0,0 +1,23 @@ + + + + sciurus17_examples_py + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + rclpy + std_msgs + moveit + moveit_py + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/sciurus17_examples_py/resource/sciurus17_examples_py b/sciurus17_examples_py/resource/sciurus17_examples_py new file mode 100644 index 00000000..e69de29b diff --git a/sciurus17_examples_py/sciurus17_examples_py/__init__.py b/sciurus17_examples_py/sciurus17_examples_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py new file mode 100755 index 00000000..b732b646 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_left_arm.py @@ -0,0 +1,70 @@ +import time + +# generic ros libraries +import rclpy +from rclpy.logging import get_logger + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + MultiPipelinePlanRequestParameters, +) + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + """Helper function to plan and execute a motion.""" + # plan to goal + logger.info("Planning trajectory") + if multi_plan_parameters is not None: + plan_result = planning_component.plan( + multi_plan_parameters=multi_plan_parameters + ) + elif single_plan_parameters is not None: + plan_result = planning_component.plan( + single_plan_parameters=single_plan_parameters + ) + else: + plan_result = planning_component.plan() + + # execute the plan + if plan_result: + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + robot.execute(robot_trajectory, controllers=[]) + else: + logger.error("Planning failed") + + time.sleep(sleep_time) + +def main(args=None): + rclpy.init(args=args) + logger = get_logger("moveit_py.pick_and_place_left_arm") + + # instantiate MoveItPy instance and get planning component + sciurus17 = MoveItPy(node_name="pick_and_place_left_arm") + l_arm = sciurus17.get_planning_component("l_arm_group") + logger.info("MoveItPy instance created") + + # set plan start state using predefined state + l_arm.set_start_state_to_current_state() + + # set pose goal using predefined state + l_arm.set_goal_state(configuration_name="l_arm_init_pose") + + # plan to goal + plan_and_execute(sciurus17, l_arm, logger, sleep_time=3.0) + + # related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/sciurus17_examples_py/setup.cfg b/sciurus17_examples_py/setup.cfg new file mode 100644 index 00000000..8ea28b93 --- /dev/null +++ b/sciurus17_examples_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/sciurus17_examples_py +[install] +install_scripts=$base/lib/sciurus17_examples_py diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py new file mode 100644 index 00000000..c5b5772c --- /dev/null +++ b/sciurus17_examples_py/setup.py @@ -0,0 +1,30 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'sciurus17_examples_py' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ubuntu', + maintainer_email='ubuntu@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', + ], + }, +) diff --git a/sciurus17_examples_py/test/test_copyright.py b/sciurus17_examples_py/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/sciurus17_examples_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/sciurus17_examples_py/test/test_flake8.py b/sciurus17_examples_py/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/sciurus17_examples_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/sciurus17_examples_py/test/test_pep257.py b/sciurus17_examples_py/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/sciurus17_examples_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/sciurus17_moveit_config/config/moveit_cpp.yaml b/sciurus17_moveit_config/config/moveit_cpp.yaml new file mode 100644 index 00000000..c439aef7 --- /dev/null +++ b/sciurus17_moveit_config/config/moveit_cpp.yaml @@ -0,0 +1,26 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: ["ompl"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + +ompl_rrtc: # Namespace for individual plan request + plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp + planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem + planning_pipeline: ompl # Name of the pipeline that is being used + planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline + max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned