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