def tourn_off_arm(): ar = ArbotiX() print "Disabling servos please wait..." #Set all servos torque limits to 0 for j in xrange(1, 7): ar.setTorqueLimit(j, 0) print("Servos disabled. Windowx_3link node closed.")
def main(): CONV_H = 0.37 CONV_V = 0.3 dyna_center = 512 arby = ArbotiX("/dev/ttyUSB0", 115200) pan_rng, tilt_rng = panorama.get_camera_angles(dyna_center, CONV_V, CONV_H) rospy.init_node('brinkmanship_core') signal(SIGINT, handler) camera_pub = rospy.Publisher('image_capture_message', String, queue_size=1) sub = rospy.Subscriber('edge_alert', String, edge_alert_callback) speed = 0.75 turn = 1 # bot_stop(0,0) # time.sleep(2) # bot_forward(speed, turn) # time.sleep(2) # bot_stop(0,0) panorama.set_camera_pose(arby, dyna_center, dyna_center - 51) bot_forward(speed, turn) flag = 0 while not rospy.is_shutdown(): if not alert: pass else: bot_stop(speed, turn) b = datetime.datetime.now() if flag == 0: print("Alert has been published") print("Time required for stopping {} microseconds".format( (b - a).microseconds)) print("Moving Pan Tilt Motors") for pan in pan_rng: for tilt in tilt_rng: panorama.set_camera_pose(arby, pan, tilt) time.sleep(3) camera_pub.publish(str(pan) + '_' + str(tilt)) flag = 1
def __init__(self, name, host, password, graphplan, actionplan, knowledge): self.communicator = Communicator() print "will init rosnode with name: " + name rospy.init_node(name) # set the speed of each servo device = ArbotiX("/dev/ttyUSB0") for i in range(0, 5): device.setSpeed(i, 20) self.arm = ArmPub(name) self.arm.goToPosition(ArmPub.POSITION_RELAX) behaviours = self.communicator.getBehaviours() behaviours.append(SystemCore()) # does not use the overwritten SpadeAgent # calls directly the constructor of Spade Agent Agent.__init__(self, name, host, password, graphplan, actionplan, behaviours) with open(knowledge) as knowledge: self.setData("knowledge", json.load(knowledge)[self.localName]) self.setData("goals", {"pose": {}}) self.setData("naoAid", spade.AID.aid("nao1@" + host, ["xmpp://nao1@" + host])) other = self.getData("knowledge")["other"] + "@" + host otherAid = spade.AID.aid(other, ["xmpp://" + other]) self.setData("otherTurtleAid", otherAid) self.setData("otherState", Goals.otherStatus["nonReady"]) print self.getData("knowledge") # stores parameters received by waitMessageBehaviour # in case it is a plan self.plan = "" # stores the parameters of an order self.parameters = list() # stores who gave the order self.messageSender = "" self.name = name
def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'): super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached) self._redist_rate = rospy.Rate(50) self._arbotix = ArbotiX('/dev/ttyUSB0') assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" self._joint_lock = Lock() self._angles, self._velocities = {}, {} rospy.Subscriber("/joint_states", JointState, self._joint_callback) time.sleep(1) self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]] self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) p.connect(p.DIRECT) widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf' self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi])) self._n_errors = 0
def __init__(self, port="/dev/ttyUSB0"): # creating an ArbotiX device (the arm) on port (normally "/dev/ttyUSB0") self.device = ArbotiX(port) joint_defaults = getJointsFromURDF() self.servos = list() # list of the joints joints = rospy.get_param('/arbotix/joints', dict()) for name in sorted(joints.keys()): # pull data to convert from Angle to ticks n = "/arbotix/joints/"+ name +"/" ticks = rospy.get_param(n+"ticks", 1024) neutral = rospy.get_param(n+"neutral", ticks/2) invert = rospy.get_param(n+"invert", False) ranges = rospy.get_param(n+"range", 300) rad_per_tick = radians(ranges)/ticks # pull min and max angles for each servo min_angle, max_angle = getJointLimits(name, joint_defaults) serv = Servo(name, min_angle, max_angle, ticks, neutral, rad_per_tick, invert) self.servos.append(serv)
def panorama_method(): CONV_H = 0.37 CONV_V = 0.3 dyna_center = 512 arby = ArbotiX("/dev/ttyUSB0", 115200) pan_rng, tilt_rng = panorama.get_camera_angles(dyna_center, CONV_V, CONV_H) rospy.init_node('brinkmanship_core') signal(SIGINT, handler) camera_pub = rospy.Publisher('image_capture_message', String, queue_size=1) speed = 0.75 turn = 1 panorama.set_camera_pose(arby, dyna_center, dyna_center - 51) print("Alert has been published") print("Moving Pan Tilt Motors") for pan in pan_rng: for tilt in tilt_rng: panorama.set_camera_pose(arby, pan, tilt) time.sleep(3) camera_pub.publish(str(pan) + '_' + str(tilt)) flag = 1
if ('Utah_Pit' in file_locations['robot_simulation_env']): TIME_OUT = 226*3+ 162*12+ 355*3+ 500*5-300 #TIME_OUT = 2200*4.1 #one shot 2200*1.3, #3 shots = 2200*3 if ('Utah_BIG' in file_locations['robot_simulation_env']): TIME_OUT = 2200*1 #normal = 1.3 big = 1 if ('Lacus_Mortis_Pit' in file_locations['robot_simulation_env']): TIME_OUT = 2200*10 if ('Pit_Edge_Test' in file_locations['robot_simulation_env']): TIME_OUT = 4*160+160 #normal = 1.3 big = 1 else: # sys.path.insert(1,file_locations['robot_simulation_env'][0:file_locations['robot_simulation_env'].rfind("/",0,-1)]+"/Brinkmanship") # from cloud_process import CloudSubscriber # from brinkmanship import Brinkmanship from arbotix_python.arbotix import ArbotiX TIME_OUT = 80+3*160+240 + rospy.get_rostime().secs arb = ArbotiX("/dev/ttyUSB0",115200) arb = ArbotiX("/dev/ttyUSB1",115200) img_capture_pub = rospy.Publisher('/image_number', Int32, queue_size = 10) map_resolution = rospy.get_param("/resolution") halfway_point = len(smach_helper.read_csv_around_pit(file_locations['file_around_pit'],map_resolution))/2 waypoint_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size = 1) pit_edge_dist_pub = rospy.Publisher('/robot_at_edge_position', Odometry, queue_size = 1) move_base_client = actionlib.SimpleActionClient('move_base',MoveBaseAction) sub_where_to_see = rospy.Subscriber('where_to_see',Float32,smach_helper.update_sun)
ticks = serv.angleToTicks(0.7854) print ticks device.setPosition(idserv, ticks) def shutdown(self): # stop turtlebot rospy.loginfo("Stop TurtleBot") # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot self.cmd_vel.publish(Twist()) # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script rospy.sleep(1) if __name__ == "__main__": rospy.init_node('arm') d = ArbotiX(port="/dev/ttyUSB0") arm = Arm() arm.newMoveServo(d, 2) ''' # premiere methode... pas mal mais n'arrive pas a fixer la vitesse arm = Arm() positions = [0,0,0,0,0,0] forward = 5 turn = 5 pos = 0.0 for i in range(0,10): arm.moveServo(pos) time.sleep(0.5) pos = pos + 0.1
from arbotix_python.arbotix import ArbotiX SERVO_IDS = [1, 2, 3, 4, 5, 6] arbotix = ArbotiX('/dev/ttyUSB0') err = arbotix.syncWrite(14, [[servo_id, 255, 1] for servo_id in SERVO_IDS]) if err != -1: print('Successfully reconfigured servos') else: print( 'Error writing to servos -- make sure start.sh is not running in the background' )
#!/usr/bin/env python # license removed for brevity import rospy from arbotix_python.arbotix import ArbotiX from arbotix_python.ax12 import * from sensor_msgs.msg import Joy turret = ArbotiX() def scan(): # Set speed print turret.setSpeed(1, 50) print turret.setSpeed(2, 50) print turret.getSpeed(1) # Put in initial position turret.setPosition(1, 510) turret.setPosition(2, 510) while turret.isMoving(1) or turret.isMoving(2): rospy.sleep(0.5) raw_input("Please enter something") # Do a scan cycle currentPosition = 0 for value in range(400, 600, 50): turret.setPosition(2, value) while turret.isMoving(2): rospy.sleep(0.5) if currentPosition == 0:
# Comment or uncomment prints for reading or writing servo parameters. from arbotix_python.arbotix import ArbotiX from time import sleep port = "/dev/ttyUSB0" arbotix = ArbotiX(port) ### To read one parameter of (one/a lot) servo/s. ### for info visit http://emanual.robotis.com/docs/en/dxl/ax/ax-12a/#control-table-of-eeprom-area # List of servos IDs servos = [1, 2, 3, 4, 5] #servos = [5] # Value of the register (EEPROM or RAM area) to read # parameter to set and its size data_name = 36 size = 2 while True: print arbotix.syncRead(servos, data_name, size) sleep(0.1) #print arbotix.syncRead(servos,data_name,size) # Return example (with Max Torque): # >>> [100, 1, 255, 3, 100, 1, 100, 1, 100, 1] # It means, Servo 1 has a value of 256*1 + 100 # Servo 2 has a value of 256*3 + 255... #----------------------------------------