def OnMsg(self, msg):
for i in range(len(msg.status)):
if (msg.status[i].name == 'HysteresisTest'):
if (msg.status[i].level == 0):
self.Log(msg.status[i].message)
self.response.status.append(msg.status[i])
if (self.count < self.testcount + 1):
mechanism.kill_controller('test_controller')
self.OpenXml()
self.Log("Starting forearm Test %s" % (self.count))
mechanism.spawn_controller(self.xml)
self.count = self.count + 1
else:
print "done"
self.finished = True
else:
self.Log(msg.status[i].message)
self.response.status.append(msg.status[i])
self.finished = True
elif (msg.status[i].name == 'EtherCAT Master'
and self.ready == False):
self.ready = True
self.OpenXml()
self.Log("Starting forearm Test 1")
mechanism.spawn_controller(self.xml)
self.count = self.count + 1
if self.finished:
time.sleep(2)
for v in self.data_dict:
print v
mechanism.shutdown()
self.topic.unregister()
self.data_topic.unregister()
self.roslaunch.stop()
wx.CallAfter(self.FinishTest)
def OnMsg(self, msg):
for i in range(len(msg.status)):
if(msg.status[i].name=='MotorTest'):
if(msg.status[i].level==0):
self.Log(msg.status[i].message)
self.response.status.append(msg.status[i])
if(self.count<self.testcount+1):
mechanism.kill_controller('test_controller')
self.OpenXml()
self.Log("Starting Motor Test %s" % (self.count))
mechanism.spawn_controller(self.xml)
self.count=self.count+1
else:
self.finished=True
else:
self.Log(msg.status[i].message)
self.response.status.append(msg.status[i])
self.finished=True
elif(msg.status[i].name=='EtherCAT Master' and self.ready==False):
self.ready=True
self.OpenXml()
self.Log("Starting Motor Test 1")
mechanism.spawn_controller(self.xml)
self.count=self.count+1
if self.finished:
mechanism.shutdown()
self.topic.unregister()
self.roslaunch.stop()
wx.CallAfter(self.Done, self.response)
def OnMsg(self, msg):
for i in range(len(msg.status)):
if(msg.status[i].name=='HysteresisTest'):
if(msg.status[i].level==0):
self.Log(msg.status[i].message)
self.response.status.append(msg.status[i])
if(self.count<self.testcount+1):
mechanism.kill_controller('test_controller')
self.OpenXml()
self.Log("Starting forearm Test %s" % (self.count))
mechanism.spawn_controller(self.xml)
self.count=self.count+1
else:
print "done"
self.finished=True
else:
self.Log(msg.status[i].message)
self.response.status.append(msg.status[i])
self.finished=True
elif(msg.status[i].name=='EtherCAT Master' and self.ready==False):
self.ready=True
self.OpenXml()
self.Log("Starting forearm Test 1")
mechanism.spawn_controller(self.xml)
self.count=self.count+1
if self.finished:
time.sleep(2)
for v in self.data_dict:
print v
mechanism.shutdown()
self.topic.unregister()
self.data_topic.unregister()
self.roslaunch.stop()
wx.CallAfter(self.FinishTest)
def OnMsg(self, msg):
for i in range(len(msg.status)):
if (msg.status[i].name == 'MotorTest'):
if (msg.status[i].level == 0):
self.Log(msg.status[i].message)
self.response.status.append(msg.status[i])
if (self.count < self.testcount + 1):
mechanism.kill_controller('test_controller')
self.OpenXml()
self.Log("Starting Motor Test %s" % (self.count))
mechanism.spawn_controller(self.xml)
self.count = self.count + 1
else:
self.finished = True
else:
self.Log(msg.status[i].message)
self.response.status.append(msg.status[i])
self.finished = True
elif (msg.status[i].name == 'EtherCAT Master'
and self.ready == False):
self.ready = True
self.OpenXml()
self.Log("Starting Motor Test 1")
mechanism.spawn_controller(self.xml)
self.count = self.count + 1
if self.finished:
mechanism.shutdown()
self.topic.unregister()
self.roslaunch.stop()
wx.CallAfter(self.Done, self.response)
def main():
usage = "base_swivvel.py <dist>; Yawes base left and right."
if len(sys.argv) < 2:
print usage
sys.exit(1)
distance = float(sys.argv[1])
rospy.wait_for_service('spawn_controller')
rospy.init_node('shuffle', anonymous=True)
path = roslib.packages.get_pkg_dir("pr2_default_controllers")
xml_for_base = open(path + "/base_controller.xml")
mechanism.spawn_controller(xml_for_base.read(), 1)
# Publishes velocity every 0.05s, calculates number of publishes
num_publishes = int(distance * 20 * 2)
cmd_vel = Twist()
cmd_vel.linear.x = float(0)
cmd_vel.linear.y = float(0)
cmd_vel.linear.z = float(0)
cmd_vel.angular.x = float(0)
cmd_vel.angular.y = float(0)
cmd_vel.angular.z = float(0)
base_vel = rospy.Publisher('cmd_vel', Twist)
try:
while not rospy.is_shutdown():
# Set velocity
cmd_vel.ang_vel.vz = float(0.4)
# Extra iteration adds a negative bias in controller
for i in range(0, num_publishes):
base_vel.publish(cmd_vel)
if rospy.is_shutdown():
break
sleep(0.05)
if rospy.is_shutdown():
break
cmd_vel.ang_vel.vz = float(-0.4)
for i in range(0, num_publishes):
base_vel.publish(cmd_vel)
if rospy.is_shutdown():
break
sleep(0.05)
except Exception, e:
print "Caught exception!"
print e
e
lc - List active controllers
sp - Spawn a controller using the xml passed over stdin
kl <name> - Kills the controller named <name>
shutdown - Ends whole process'''
sys.exit(exit_code)
if __name__ == '__main__':
if len(sys.argv) < 2:
print_usage()
if sys.argv[1] == 'lt':
mechanism.list_controller_types()
elif sys.argv[1] == 'lc':
mechanism.list_controllers()
elif sys.argv[1] == 'sp':
xml = ""
if len(sys.argv) > 2:
f = open(sys.argv[2])
xml = f.read()
f.close()
else:
xml = sys.stdin.read()
mechanism.spawn_controller(xml)
elif sys.argv[1] == 'kl':
for c in sys.argv[2:]:
mechanism.kill_controller(c)
elif sys.argv[1] == 'shutdown':
mechanism.shutdown()
else:
print_usage(1)
lc - List active controllers
sp - Spawn a controller using the xml passed over stdin
kl <name> - Kills the controller named <name>
shutdown - Ends whole process'''
sys.exit(exit_code)
if __name__ == '__main__':
if len(sys.argv) < 2:
print_usage()
time.sleep(2) #FIXME: added by john, this might have removed assert(robot) failure in controller initXml calls on startup. need to investigate why if any memory corruption or race condition for MC stack.
if sys.argv[1] == 'lt':
mechanism.list_controller_types()
elif sys.argv[1] == 'lc':
mechanism.list_controllers()
elif sys.argv[1] == 'sp':
xml = ""
if len(sys.argv) > 2:
f = open(sys.argv[2])
xml = f.read()
f.close()
else:
xml = sys.stdin.read()
mechanism.spawn_controller(xml)
elif sys.argv[1] == 'kl':
for c in sys.argv[2:]:
mechanism.kill_controller(c)
elif sys.argv[1] == 'shutdown':
mechanism.shutdown()
else:
print_usage(1)
import string
from time import sleep
import random
import rospy
from std_msgs import *
from pr2_mechanism_controllers.srv import *
from mechanism_control import mechanism
if __name__ == "__main__":
# spawn head test controller from xml file in pkg
controller_path = roslib.packages.get_pkg_dir(
'life_test') + '/laser_tilt_test/life_test/controllers.xml'
xml_for = open(controller_path)
mechanism.spawn_controller(xml_for.read())
# run laser controller with sine profile
# based off pr2_mechanism_controllers/control_laser.py
rospy.wait_for_service('laser_controller/set_profile')
s = rospy.ServiceProxy('laser_controller/set_profile', SetProfile)
resp = s.call(SetProfileRequest(0.0, 0.0, 0.0, 0.0, 4, 0.25, 1.25, 0.25))
rospy.init_node('hokuyo_commander', anonymous=True)
sleep(1)
try:
rospy.spin()
except Exception, e:
print 'Caught exception running controllers'
