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'