Make rosrun package, try moveit libraries

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(garbage_bot)
+
+find_package(catkin
+  REQUIRED
+  COMPONENTS
+  rospy
+  xacro
+  actionlib
+  sensor_msgs
+  control_msgs
+  trajectory_msgs
+  cv_bridge
+  dynamic_reconfigure
+  baxter_core_msgs
+  baxter_interface
+)
+
+catkin_python_setup()
+
+catkin_package(
+  CATKIN_DEPENDS
+  rospy
+  xacro
+  actionlib
+  sensor_msgs
+  control_msgs
+  trajectory_msgs
+  cv_bridge
+  dynamic_reconfigure
+  baxter_core_msgs
+  baxter_interface
+)
+
+install(
+  DIRECTORY scripts/
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+)
+
+install(
+  DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  USE_SOURCE_PERMISSIONS
+)
+
+install(
+  DIRECTORY share/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/share
+  USE_SOURCE_PERMISSIONS
+)

package.xml

+<?xml version="1.0"?>
+<package>
+  <name>garbage_bot</name>
+  <version>0.0.1</version>
+  <description>
+    The Garbage Bot
+  </description>
+
+  <license>MIT</license>
+  <maintainer email="mathieu.reymond@vub.be">
+    Mathieu Reymond
+  </maintainer>
+  <author>Garbage Bot Inc.</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+  <build_depend>xacro</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>control_msgs</build_depend>
+  <build_depend>trajectory_msgs</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>baxter_core_msgs</build_depend>
+  <build_depend>baxter_interface</build_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>xacro</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>control_msgs</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>baxter_core_msgs</run_depend>
+  <run_depend>baxter_interface</run_depend>
+
+</package>

setup.py

+#!/usr/bin/env python
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup()
+d['packages'] = ['test']
+d['package_dir'] = {'': 'src'}
+
+setup(**d)

src/test/moveit_python_test.py

+#!/usr/bin/env python
+import rospy
+from moveit_python import MoveGroupInterface, PlanningSceneInterface
+
+rospy.init_node('moveit_py')
+g = MoveGroupInterface('left_arm', '/world')
+
+# create a planning scene interface, provide name of root link
+p = PlanningSceneInterface('world')
+
+# add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame
+p.addCube('my_cube', 0.1, 1, 0, 0.5)
+
+joints = ['left_w0', 'left_w1', 'left_w2', 'left_e0', 'left_e1', 'left_s0', 'left_s1']
+positions = [0.6699952259595108,
+             1.030009435085784,
+             -0.4999997247485215,
+             -1.189968899785275,
+             1.9400238130755056,
+             -0.08000397926829805,
+             -0.9999781166910306]
+
+g.moveToJointPosition(joints, positions)

src/test/moveit_robots_test.py

+#!/usr/bin/env python
+import sys
+import copy
+import rospy
+import moveit_commander
+import moveit_msgs.msg
+import geometry_msgs.msg
+
+moveit_commander.roscpp_initialize(sys.argv)
+rospy.init_node('move_group_python_interface_tutorial',
+                anonymous=True)
+
+robot = moveit_commander.RobotCommander()
+scene = moveit_commander.PlanningSceneInterface()
+
+print(robot.get_current_state())
+print(robot.get_group_names())
+group = moveit_commander.MoveGroupCommander('left_arm')
+
+pose_target = geometry_msgs.msg.Pose()
+pose_target.orientation.w = 1.0
+pose_target.position.x = 0.7
+pose_target.position.y = -0.05
+pose_target.position.z = 1.1
+group.set_pose_target(pose_target)
+
+plan1 = group.plan()
+group.go()

Code/test.py → src/test/test.py

+#!/usr/bin/env python
 import struct
 import os
 import rospy
@@ -65,7 +66,8 @@ def move(limb, pose):
     limb.move_to_joint_positions(joints)
 
 def print_msg(msg):
-    print(msg)
+    pass
+    # print(msg)
 
 def take_snapshot():
     subscriber = rospy.Subscriber('cameras/left_hand_camera/image', Image, print_msg)
@@ -111,4 +113,10 @@ if __name__ == '__main__':
                              z=0.0,
                              w=0.2)
     move('left', Pose(position=Point(x=0.8, y=0.0, z=0.5), orientation=overhead_orientation))
-    take_snapshot()
+    # overhead_orientation = Quaternion(
+    #                          x=0.0,
+    #                          y=1.0,
+    #                          z=0.0,
+    #                          w=0.0)
+    # move('left', Pose(position=Point(x=0.8, y=0.0, z=0.5), orientation=overhead_orientation))
+    # take_snapshot()