00001
00002
00003 import roslib
00004 roslib.load_manifest('ipa_canopen_tutorials')
00005 import rospy
00006 import copy
00007 from cob_srvs.srv import Trigger
00008 from brics_actuator.msg import JointVelocities
00009 from brics_actuator.msg import JointValue
00010
00011 rospy.init_node("ipa_canopen_test")
00012
00013 rospy.wait_for_service('/tray_controller/init')
00014 print "found init"
00015 initService = rospy.ServiceProxy('/tray_controller/init', Trigger)
00016 resp = initService()
00017
00018 velPublisher = rospy.Publisher('/tray_controller/command_vel', JointVelocities)
00019 rospy.sleep(2.0)
00020 v = JointVelocities()
00021 vv1 = JointValue()
00022 vv1.timeStamp = rospy.Time.now()
00023 vv1.joint_uri = "tray_1_joint"
00024 vv2 = copy.deepcopy(vv1)
00025 vv2.joint_uri = "tray_2_joint"
00026 vv3 = copy.deepcopy(vv1)
00027 vv3.joint_uri = "tray_3_joint"
00028 v.velocities = [vv1,vv2,vv3]
00029
00030 v.velocities[0].value = 0.0
00031
00032 while not rospy.is_shutdown():
00033 v.velocities[0].value = 0.05
00034 v.velocities[1].value = 0.05
00035 velPublisher.publish(v)
00036
00037 rospy.sleep(1.0)
00038
00039 v.velocities[0].value = 0
00040 v.velocities[1].value = 0
00041 velPublisher.publish(v)
00042
00043 rospy.sleep(1.0)
00044
00045 v.velocities[0].value = - 0.05
00046 v.velocities[1].value = - 0.05
00047 velPublisher.publish(v)
00048
00049 rospy.sleep(1.0)
00050
00051 v.velocities[0].value = 0
00052 v.velocities[1].value = 0
00053 velPublisher.publish(v)
00054
00055 rospy.sleep(1.0)
