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 == '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 == '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)
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)
def __del__(self): print 'Destroying controller %s...' % self._name mechanism.kill_controller(self._name)
name="cal_torso" topic="cal_torso">
<calibrate joint="torso_lift_joint"
actuator="torso_lift_motor"
transmission="torso_lift_trans"
velocity="2.0" />
<pid p="20000" i="25" d="0" iClamp="1000" />
</controller>'''
def main():
print 'waiting'
rospy.wait_for_service('spawn_controller')
rospy.init_node('cal_torso', anonymous=True)
try:
print 'Spawning controller'
resp = spawn_controller(xml_for())
sleep(1)
print 'Spawned'
print resp.name[0]
print ord(resp.ok[0])
print resp.ok
rospy.spin()
except Exception, e:
print e
e
finally:
mechanism.kill_controller('cal_torso')
if __name__ == "__main__":
main()
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)
<controller name=\"arm_effort_controller\" type=\"JointEffortControllerNode\">\
<joint name=\"r_elbow_flex_joint\" />\
</controller>"
if __name__ == "__main__":
rospy.init_node('arm_impact_test')
rospy.wait_for_service('spawn_controller')
spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
resp = spawn_controller(xml_for())
if len(resp.ok) < 1 or not resp.ok[0]:
print "Failed to spawn controller"
sys.exit(1)
pub = rospy.Publisher("arm_effort_controller/set_command", Float64)
effort = 15
try:
while not rospy.is_shutdown():
pub.publish(Float64(effort))
sleep(1)
effort = effort * -1
finally:
for i in range(1, 3):
try:
mechanism.kill_controller('arm_effort_controller')
except:
print 'Failed to kill arm life controller'
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'
print e
finally:
print 'Killing controllers'
for i in range(1, 3):
try:
mechanism.kill_controller('laser_controller')
except:
print 'Failed to kill laser controller'
lt - List controller Types
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':
mechanism.kill_controller(sys.argv[2])
elif sys.argv[1] == 'shutdown':
mechanism.shutdown()
else:
print_usage(1)
def __del__(self):
print 'Destroying controller %s...' % self._name
mechanism.kill_controller(self._name)
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
finally:
# Set velocity = 0
cmd_vel.ang_vel.vz = float(0)
base_vel.publish(cmd_vel)
mechanism.kill_controller('base_controller')
if __name__ == '__main__':
main()
