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...
#----------------------------------------
