Python3 migration

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)
+add_subdirectory(doc/moveit_cpp)
+add_subdirectory(doc/collision_environments)

doc/collision_environments/CMakeLists.txt

@@ -0,0 +1,4 @@
+catkin_install_python(PROGRAMS
+  scripts/collision_scene_example.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

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()

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}
 )

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: