From 42d68a073ec2ebfdd817a1882be6960899df0a4d Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sat, 3 Oct 2020 13:30:05 -0600 Subject: [PATCH 1/2] Python3 migration of move_group_python_interface_tutorial --- .../CMakeLists.txt | 2 +- .../move_group_python_interface_tutorial.py | 63 +++++++++---------- 2 files changed, 29 insertions(+), 36 deletions(-) diff --git a/doc/move_group_python_interface/CMakeLists.txt b/doc/move_group_python_interface/CMakeLists.txt index dc818b050..5e27dcf63 100644 --- a/doc/move_group_python_interface/CMakeLists.txt +++ b/doc/move_group_python_interface/CMakeLists.txt @@ -1,4 +1,4 @@ -install(PROGRAMS +catkin_install_python(PROGRAMS scripts/move_group_python_interface_tutorial.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py index 1eaed1ed3..5dcda4d1f 100755 --- a/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py +++ b/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py @@ -41,6 +41,10 @@ ## and a `RobotCommander`_ class. More on these below. We also import `rospy`_ and some messages that we will use: ## +# Python 2/3 compatibility imports +from __future__ import print_function +from six.moves import input + import sys import copy import rospy @@ -119,21 +123,21 @@ def __init__(self): ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # We can get the name of the reference frame for this robot: planning_frame = move_group.get_planning_frame() - print "============ Planning frame: %s" % planning_frame + print("============ Planning frame: %s" % planning_frame) # We can also print the name of the end-effector link for this group: eef_link = move_group.get_end_effector_link() - print "============ End effector link: %s" % eef_link + print("============ End effector link: %s" % eef_link) # We can get a list of all the groups in the robot: group_names = robot.get_group_names() - print "============ Available Planning Groups:", robot.get_group_names() + print("============ Available Planning Groups:", robot.get_group_names()) # Sometimes for debugging it is useful to print the entire state of the # robot: - print "============ Printing robot state" - print robot.get_current_state() - print "" + print("============ Printing robot state") + print(robot.get_current_state()) + print("") ## END_SUB_TUTORIAL # Misc variables @@ -449,58 +453,47 @@ def remove_box(self, timeout=4): def main(): try: - print "" - print "----------------------------------------------------------" - print "Welcome to the MoveIt MoveGroup Python Interface Tutorial" - print "----------------------------------------------------------" - print "Press Ctrl-D to exit at any time" - print "" - print "============ Press `Enter` to begin the tutorial by setting up the moveit_commander ..." - raw_input() + print("") + print("----------------------------------------------------------") + print("Welcome to the MoveIt MoveGroup Python Interface Tutorial") + print("----------------------------------------------------------") + print("Press Ctrl-D to exit at any time") + print("") + input("============ Press `Enter` to begin the tutorial by setting up the moveit_commander ...") tutorial = MoveGroupPythonIntefaceTutorial() - print "============ Press `Enter` to execute a movement using a joint state goal ..." - raw_input() + input("============ Press `Enter` to execute a movement using a joint state goal ...") tutorial.go_to_joint_state() - print "============ Press `Enter` to execute a movement using a pose goal ..." - raw_input() + input("============ Press `Enter` to execute a movement using a pose goal ...") tutorial.go_to_pose_goal() - print "============ Press `Enter` to plan and display a Cartesian path ..." - raw_input() + input("============ Press `Enter` to plan and display a Cartesian path ...") cartesian_plan, fraction = tutorial.plan_cartesian_path() - print "============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ..." - raw_input() + input("============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ...") tutorial.display_trajectory(cartesian_plan) - print "============ Press `Enter` to execute a saved path ..." - raw_input() + input("============ Press `Enter` to execute a saved path ...") tutorial.execute_plan(cartesian_plan) - print "============ Press `Enter` to add a box to the planning scene ..." - raw_input() + input("============ Press `Enter` to add a box to the planning scene ...") tutorial.add_box() - print "============ Press `Enter` to attach a Box to the Panda robot ..." - raw_input() + input("============ Press `Enter` to attach a Box to the Panda robot ...") tutorial.attach_box() - print "============ Press `Enter` to plan and execute a path with an attached collision object ..." - raw_input() + input("============ Press `Enter` to plan and execute a path with an attached collision object ...") cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1) tutorial.execute_plan(cartesian_plan) - print "============ Press `Enter` to detach the box from the Panda robot ..." - raw_input() + input("============ Press `Enter` to detach the box from the Panda robot ...") tutorial.detach_box() - print "============ Press `Enter` to remove the box from the planning scene ..." - raw_input() + input("============ Press `Enter` to remove the box from the planning scene ...") tutorial.remove_box() - print "============ Python tutorial demo complete!" + print("============ Python tutorial demo complete!") except rospy.ROSInterruptException: return except KeyboardInterrupt: From 01ffbf5d4e2462eb928d5309feeb54f5c28a671f Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Sun, 4 Oct 2020 13:54:48 -0600 Subject: [PATCH 2/2] Python3 migration of collision_enviroments --- CMakeLists.txt | 3 ++- doc/collision_environments/CMakeLists.txt | 4 ++++ .../scripts/collision_scene_example.py | 14 +++++++++----- 3 files changed, 15 insertions(+), 6 deletions(-) create mode 100644 doc/collision_environments/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 089057166..30540defa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,4 +67,5 @@ add_subdirectory(doc/tests) add_subdirectory(doc/controller_configuration) add_subdirectory(doc/trajopt_planner) add_subdirectory(doc/creating_moveit_plugins/lerp_motion_planner) -add_subdirectory(doc/moveit_cpp) \ No newline at end of file +add_subdirectory(doc/moveit_cpp) +add_subdirectory(doc/collision_environments) \ No newline at end of file diff --git a/doc/collision_environments/CMakeLists.txt b/doc/collision_environments/CMakeLists.txt new file mode 100644 index 000000000..3148de890 --- /dev/null +++ b/doc/collision_environments/CMakeLists.txt @@ -0,0 +1,4 @@ +catkin_install_python(PROGRAMS + scripts/collision_scene_example.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/doc/collision_environments/scripts/collision_scene_example.py b/doc/collision_environments/scripts/collision_scene_example.py index 4710f0013..35eb0221f 100755 --- a/doc/collision_environments/scripts/collision_scene_example.py +++ b/doc/collision_environments/scripts/collision_scene_example.py @@ -1,4 +1,8 @@ #!/usr/bin/env python + +# Python 2/3 compatibility import +from __future__ import print_function + import rospy from moveit_commander import RobotCommander, PlanningSceneInterface import geometry_msgs.msg @@ -16,7 +20,7 @@ def __init__(self): self.robot = RobotCommander() # pause to wait for rviz to load - print "============ Waiting while RVIZ displays the scene with obstacles..." + print("============ Waiting while RVIZ displays the scene with obstacles...") # TODO: need to replace this sleep by explicitly waiting for the scene to be updated. rospy.sleep(2) @@ -27,7 +31,7 @@ def add_one_box(self): self.add_box_object("box1", box1_dimensions, box1_pose) - print "============ Added one obstacle to RViz!!" + print("============ Added one obstacle to RViz!!") def add_four_boxes(self): box1_pose = [0.20, 0.50, 0.25, 0, 0, 0, 1] @@ -47,7 +51,7 @@ def add_four_boxes(self): self.add_box_object("box3", box3_dimensions, box3_pose) self.add_box_object("box4", box4_dimensions, box4_pose) - print "========== Added 4 obstacles to the scene!!" + print("========== Added 4 obstacles to the scene!!") def add_box_object(self, name, dimensions, pose): p = geometry_msgs.msg.PoseStamped() @@ -70,12 +74,12 @@ def add_box_object(self, name, dimensions, pose): load_scene = CollisionSceneExample() if (len(sys.argv) != 2): - print "Correct usage:: \n\"rosrun moveit_tutorials collision_scene_example.py cluttered\" OR \n\"rosrun moveit_tutorials collision_scene_example.py sparse\"" + print("Correct usage:: \n\"rosrun moveit_tutorials collision_scene_example.py cluttered\" OR \n\"rosrun moveit_tutorials collision_scene_example.py sparse\"") sys.exit() if sys.argv[1] == "cluttered": load_scene.add_four_boxes(); elif sys.argv[1] == "sparse": load_scene.add_one_box(); else: - print "Please specify correct type of scene as cluttered or sparse" + print("Please specify correct type of scene as cluttered or sparse") sys.exit()