From b80158e7d9018cb440e450d55f44650de8218064 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 20 Aug 2024 11:46:11 +0900 Subject: [PATCH] =?UTF-8?q?utils=E3=82=92=E5=88=86=E9=9B=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples_py/gripper_control.py | 30 +----------------- .../pick_and_place_left_arm.py | 31 +------------------ .../sciurus17_examples_py/utils.py | 30 ++++++++++++++++++ 3 files changed, 32 insertions(+), 59 deletions(-) create mode 100644 sciurus17_examples_py/sciurus17_examples_py/utils.py diff --git a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py index 1f259e7..3e98188 100755 --- a/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py +++ b/sciurus17_examples_py/sciurus17_examples_py/gripper_control.py @@ -28,35 +28,7 @@ PlanRequestParameters, ) - -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.""" - 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) +from sciurus17_examples_py.utils import plan_and_execute def main(args=None): 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 index 8f535b2..92ddc42 100755 --- 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 @@ -28,36 +28,7 @@ PlanRequestParameters, ) - -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.""" - 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) - +from sciurus17_examples_py.utils import plan_and_execute def main(args=None): rclpy.init(args=args) diff --git a/sciurus17_examples_py/sciurus17_examples_py/utils.py b/sciurus17_examples_py/sciurus17_examples_py/utils.py new file mode 100644 index 0000000..95903dc --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/utils.py @@ -0,0 +1,30 @@ +import time + +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.""" + 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)