M. Stokroos
3 years ago
8 changed files with 581 additions and 4 deletions
@ -0,0 +1,191 @@
@@ -0,0 +1,191 @@
|
||||
#!/usr/bin/env python2 |
||||
|
||||
# nog geen 3 omdat serial er nog niet bij zit. (nog steeds?) |
||||
|
||||
# To control Nexus robot car |
||||
# Via /dev/ttyUSB? |
||||
# |
||||
|
||||
import sys, os, serial, time, traceback |
||||
from struct import pack, unpack |
||||
import numpy as np |
||||
import re |
||||
|
||||
def key_description(character): |
||||
"""generate a readable description for a key""" |
||||
ascii_code = ord(character) |
||||
if ascii_code < 32: |
||||
return 'Ctrl+%c' % (ord('@') + ascii_code) |
||||
else: |
||||
return repr(character) |
||||
|
||||
# voorlopig alleen posix |
||||
if os.name == 'posixnee': |
||||
import termios, sys, os |
||||
class Console: |
||||
def __init__(self): |
||||
self.fd = sys.stdin.fileno() |
||||
|
||||
def setup(self): |
||||
self.old = termios.tcgetattr(self.fd) |
||||
new = termios.tcgetattr(self.fd) |
||||
# new[3] = new[3] & ~termios.ICANON & ~termios.ECHO & ~termios.ISIG |
||||
new[3] = new[3] & ~termios.ICANON # & ~termios.ISIG |
||||
new[6][termios.VMIN] = 1 |
||||
new[6][termios.VTIME] = 0 |
||||
termios.tcsetattr(self.fd, termios.TCSANOW, new) |
||||
|
||||
# Uiteindelijk willen we meer tegelijk lezen.. |
||||
# zeker als er grotere hoeveelheden data verstuurd worden door het target |
||||
def getkey(self): |
||||
c = os.read(self.fd, 1) |
||||
return c |
||||
|
||||
def cleanup(self): |
||||
termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old) |
||||
|
||||
console = Console() |
||||
|
||||
def cleanup_console(): |
||||
console.cleanup() |
||||
|
||||
console.setup() |
||||
sys.exitfunc = cleanup_console #terminal modes have to be restored on exit... |
||||
|
||||
#else: |
||||
# raise NotImplementedError( "Sorry no implementation for your platform (%s) available." % sys.platform) |
||||
|
||||
|
||||
CONVERT_CRLF = 2 |
||||
CONVERT_CR = 1 |
||||
CONVERT_LF = 0 |
||||
NEWLINE_CONVERISON_MAP = ('\n', '\r', '\r\n') |
||||
LF_MODES = ('LF', 'CR', 'CR/LF') |
||||
|
||||
REPR_MODES = ('raw', 'some control', 'all control', 'hex') |
||||
|
||||
class CarControl: |
||||
def __init__(self, port, baudrate, parity, rtscts, xonxoff, echo=False, convert_outgoing=CONVERT_CRLF, repr_mode=0): |
||||
self.serial = serial.Serial(port, baudrate, parity=parity, rtscts=rtscts, xonxoff=xonxoff, timeout = 1) |
||||
self.echo = echo |
||||
self.repr_mode = repr_mode |
||||
self.convert_outgoing = convert_outgoing |
||||
self.newline = NEWLINE_CONVERISON_MAP[self.convert_outgoing] |
||||
self.dtr_state = True |
||||
self.rts_state = True |
||||
self.break_state = False |
||||
|
||||
# Het lijkt er op of er na een power up van de Nexus twee characters (waarde 0) gezonden worden. FTDI chip? |
||||
# De writer pas wat laten doen als we daar geen last meer van hebben. |
||||
tmp = self.serial.read(1) |
||||
#print 'pretmp', len(tmp) |
||||
#if len(tmp) > 0: |
||||
# print ord(tmp[0]) |
||||
time.sleep(1) |
||||
tmp = self.serial.read(1) |
||||
#print 'prettmp', len(tmp) |
||||
#if len(tmp) > 0: |
||||
# print ord(tmp[0]) |
||||
|
||||
def stop(self): |
||||
print 'Waar is de stop?' |
||||
self.serial.close() |
||||
|
||||
def getinfopacket(self): |
||||
try: |
||||
d = self.serial.read(24) |
||||
print len(d) #, ord(d[0]) |
||||
if ord(d[0]) <> 2 or ord(d[23]) <> 3: # STX and ETX |
||||
print 'Packet framing error!' |
||||
else: |
||||
print 'New info packet received.' |
||||
if ord(d[1]) == 1: # scope data packet (try that first) |
||||
status_control = ord(d[3]); |
||||
error_code = ord(d[4]); |
||||
status_car = ord(d[5]); |
||||
#(sensor,) = unpack('h', d[10:12]); |
||||
(speed,) = unpack('h', d[6:8]); |
||||
(debtime,) = unpack('h', d[10:12]); |
||||
# we gebruiken dit in de write thread en bij autoreply. |
||||
# evt. korte lock bij uitpakken data |
||||
waitreq = 1 |
||||
else: |
||||
print 'Packet type %x received, not implemented.' % d[1] |
||||
print 'Info packet received with error code %x %x %d and %d' % (error_code, status_car, speed, debtime) |
||||
except: |
||||
print 'Wanneer hier?' |
||||
traceback.print_exc(file=sys.stdout) |
||||
|
||||
|
||||
""" |
||||
Packets are 8 bytes. |
||||
{ stx, command, par's ..., etx } |
||||
parameters dependant upon the command |
||||
|
||||
Nexus robot functions of type void(*function[11])(unsigned int speedMMPS) : |
||||
{goAhead, turnLeft, turnRight, rotateRight, rotateLeft, allStop, backOff, upperLeft, lowerLeft, upperRight, lowerRight}; |
||||
{ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 } |
||||
this wil go in command. |
||||
Parameter speedMMPS in par |
||||
|
||||
-700 < speedMMPS < 700 |
||||
speedMMPS = 200 komt overeen met 0.21 m/sec |
||||
maximale snelheid 0.735 m/sec |
||||
van m/s naar speedMMPS : maal 952 |
||||
|
||||
|
||||
Other commands: |
||||
12: setWheelspeedcommand (set speed of 4 wheels) |
||||
13: set speedMMPS on Nexus |
||||
20: ask info packet |
||||
a 24 byte packet will be received |
||||
21: autoshutdown off (default) |
||||
22: autoshutdown on |
||||
23: autoreply off (default) |
||||
24: autoreply on. reply with a infopacket |
||||
25: start standalone? |
||||
26: stop standalone |
||||
27: keep alive |
||||
28: repeat mode info packets off (default) |
||||
29: repeat mode info packete on |
||||
|
||||
""" |
||||
|
||||
def sendpacket(self, command, par): |
||||
outpkt = bytearray(16) |
||||
outpkt[0] = 2 # STX |
||||
outpkt[1] = command |
||||
outpkt[2:4] = pack('h', par) |
||||
outpkt[15] = 3 # ETX |
||||
self.serial.write(outpkt) # send packet |
||||
print 'sendpacket', outpkt[0], outpkt[1], outpkt[2], outpkt[3], outpkt[4], outpkt[5], outpkt[6], ' ... ', outpkt[15] |
||||
|
||||
def sendwheelscommand(self, frontleft, backleft, frontright, backright): |
||||
outpkt = bytearray(16) |
||||
outpkt[0] = 2 # STX |
||||
outpkt[1] = 12 # setwheels command |
||||
outpkt[2:4] = pack('h', frontleft) |
||||
outpkt[4:6] = pack('h', backleft) |
||||
outpkt[6:8] = pack('h', frontright) |
||||
outpkt[8:10] = pack('h', backright) |
||||
|
||||
outpkt[15] = 3 # ETX |
||||
self.serial.write(outpkt) # send packet |
||||
#print 'outpkt: ', unpack('h', outpkt[2:4]), unpack('h', outpkt[4:6]), unpack('h', outpkt[6:8]), unpack('h',outpkt[8:10]) |
||||
|
||||
# def setspeed(self, lin,rot): |
||||
# # lineaire (m/sec) and rotatie (rad/sec) snelheid |
||||
# # convert to nexus (calibrated with nexus1) |
||||
# speed = lin * 1024 |
||||
# rot = rot * 300 |
||||
# self.sendwheelscommand(speed -rot, speed - rot, speed + rot , speed + rot) |
||||
|
||||
def setspeed(self, linx, liny, rot): |
||||
# lineaire x and y (m/sec) and rotatie (rad/sec) snelheid |
||||
# convert to nexus (calibrated with nexus1) |
||||
speedx = linx * -1024 |
||||
speedy = liny * -1024 |
||||
rot = rot * 300 |
||||
self.sendwheelscommand(speedx -speedy -rot, speedx +speedy -rot, speedx -speedy +rot, speedx +speedy +rot) |
||||
|
||||
|
||||
@ -0,0 +1,220 @@
@@ -0,0 +1,220 @@
|
||||
#!/usr/bin/env python |
||||
|
||||
""" |
||||
Nexus controller |
||||
|
||||
|
||||
""" |
||||
|
||||
import rospy |
||||
import socket |
||||
from math import radians,sin,cos,pi |
||||
from std_msgs.msg import Float64 |
||||
|
||||
from diagnostic_msgs.msg import * |
||||
from geometry_msgs.msg import Quaternion |
||||
from geometry_msgs.msg import Twist |
||||
from nav_msgs.msg import Odometry |
||||
from tf.broadcaster import TransformBroadcaster |
||||
|
||||
import carcomm |
||||
|
||||
class NexusController(): |
||||
|
||||
def __init__(self, device, name): |
||||
|
||||
self.status = "OK" |
||||
self.level = DiagnosticStatus.OK |
||||
# self.name = socket.gethostname() |
||||
|
||||
self.dirty = False # newly updated position? |
||||
self.enabled = True # can we take commands? |
||||
|
||||
self.last_cmd = rospy.Time.now() |
||||
self.rate = rospy.get_param("~nexus/rate", 50.0) |
||||
self.timeout = rospy.get_param('~nexus/timeout',1.0) |
||||
self.t_delta = rospy.Duration(1.0/self.rate) |
||||
self.t_next = rospy.Time.now() + self.t_delta |
||||
self.fake = rospy.get_param("~sim", False) |
||||
port = rospy.get_param("~port", "/dev/ttyUSB0") |
||||
|
||||
# parameters? |
||||
# self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', ''+self.name+'/base_footprint') |
||||
self.base_frame_id = rospy.get_param('~controllers/'+name+'/base_frame_id', name+'base_footprint') |
||||
self.odom_frame_id = rospy.get_param('~controllers/'+name+'/odom_frame_id', 'odom') |
||||
|
||||
self.x = 0.0 # position in xy plane |
||||
self.y = 0.0 |
||||
self.th = 0 # orientation (theta) |
||||
|
||||
self.dx = 0 # x-speed in meter/sec |
||||
self.dy = 0 # y-speed in meter/sec |
||||
self.dr = 0 # rotation in radians/sec |
||||
self.dx_last = 0 # previous x-speed |
||||
self.dy_last = 0 # previous y-speed |
||||
self.dr_last = 0 # previous rotation |
||||
self.then = rospy.Time.now() # time for determining dx/dy |
||||
|
||||
# ROS interfaces |
||||
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) |
||||
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5) |
||||
self.odomBroadcaster = TransformBroadcaster() |
||||
|
||||
rospy.loginfo("Started Controller ("+name+")") |
||||
|
||||
# connect to the nexus robot |
||||
if not self.fake: |
||||
# connect to Nexus |
||||
baudrate = 28800; parity = 'N'; rtscts = False; xonxoff = False; echo = False |
||||
convert_outgoing = carcomm.CONVERT_CRLF; repr_mode = 0 |
||||
self.carcon = carcomm.CarControl(port, baudrate, parity, rtscts, xonxoff, echo, convert_outgoing, repr_mode ) |
||||
# to be sure that the car is ready |
||||
rospy.sleep(2.0) |
||||
rospy.loginfo("Started Nexus connection on port " + port + ".") |
||||
|
||||
self.carcon.sendpacket(20,0) |
||||
self.carcon.getinfopacket() |
||||
|
||||
else: |
||||
rospy.loginfo("Nexus being simulated.") |
||||
|
||||
|
||||
def startup(self): |
||||
print "startup called" |
||||
|
||||
def shutdown(self): |
||||
# close connection with the nexus |
||||
if not self.fake: |
||||
console.cleanup_console() |
||||
carcon.stop() |
||||
print "shutdown called" |
||||
|
||||
def cmdVelCb(self,req): |
||||
""" Handle movement requests. """ |
||||
self.last_cmd = rospy.Time.now() |
||||
if self.fake: |
||||
self.dx = req.linear.x # m/s |
||||
self.dy = req.linear.y # m/s |
||||
self.dr = req.angular.z # rad/s |
||||
else: |
||||
# same as fake |
||||
self.dx = req.linear.x # m/s |
||||
self.dy = req.linear.y # m/s |
||||
self.dr = req.angular.z # rad/s |
||||
|
||||
def getDiagnostics(self): |
||||
""" Get a diagnostics status. """ |
||||
msg = DiagnosticStatus() |
||||
msg.name = self.name |
||||
return msg |
||||
|
||||
def update(self): |
||||
""" Do step in nexus controller. """ |
||||
now = rospy.Time.now() |
||||
if now > self.t_next: |
||||
elapsed = now - self.then |
||||
self.then = now |
||||
elapsed = elapsed.to_sec() |
||||
|
||||
if self.fake: |
||||
self.x += cos(self.th)*self.dx*elapsed - sin(self.th)*self.dy*elapsed |
||||
self.y += sin(self.th)*self.dx*elapsed + cos(self.th)*self.dy*elapsed |
||||
self.th += self.dr*elapsed |
||||
else: |
||||
# we use read reackoning for the moment |
||||
self.x += cos(self.th)*self.dx*elapsed - sin(self.th)*self.dy*elapsed |
||||
self.y += sin(self.th)*self.dx*elapsed + cos(self.th)*self.dy*elapsed |
||||
self.th += self.dr*elapsed |
||||
|
||||
|
||||
# publish or perish |
||||
quaternion = Quaternion() |
||||
quaternion.x = 0.0 |
||||
quaternion.y = 0.0 |
||||
quaternion.z = sin(self.th/2) |
||||
quaternion.w = cos(self.th/2) |
||||
self.odomBroadcaster.sendTransform( |
||||
(self.x, self.y, 0), |
||||
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), |
||||
rospy.Time.now(), |
||||
self.base_frame_id, |
||||
self.odom_frame_id |
||||
) |
||||
|
||||
odom = Odometry() |
||||
odom.header.stamp = now |
||||
odom.header.frame_id = self.odom_frame_id |
||||
odom.pose.pose.position.x = self.x |
||||
odom.pose.pose.position.y = self.y |
||||
odom.pose.pose.position.z = 0 |
||||
odom.pose.pose.orientation = quaternion |
||||
odom.child_frame_id = self.base_frame_id |
||||
odom.twist.twist.linear.x = self.dx |
||||
odom.twist.twist.linear.y = self.dy |
||||
odom.twist.twist.angular.z = self.dr |
||||
self.odomPub.publish(odom) |
||||
|
||||
# stop on timeout. nu niet zinnig, er is toch geen terugkoppeling. |
||||
# if now > (self.last_cmd + rospy.Duration(self.timeout)): |
||||
# self.dx = 0; self.dr = 0; |
||||
# print "timeout reached" |
||||
|
||||
# update motors |
||||
if not self.fake: |
||||
# only send if it really is different! |
||||
if self.dx <> self.dx_last or self.dy <> self.dy_last or self.dr <> self.dr_last: |
||||
# voor of achteruit |
||||
# self.carcon.setspeed(-self.dx, self.dy, self.dr) |
||||
self.carcon.setspeed(self.dx, self.dy, self.dr) |
||||
self.dx_last = self.dx; self.dy_last = self.dy; self.dr_last = self.dr |
||||
|
||||
self.t_next = now + self.t_delta |
||||
|
||||
|
||||
|
||||
def getDiagnostics(self): |
||||
""" Update status of servos (voltages, temperatures). """ |
||||
print "diagnostics" |
||||
""" |
||||
if self.iter % 5 == 0 and not self.fake: |
||||
if self.device.use_sync_read: |
||||
# arbotix/servostik/wifi board sync_read |
||||
synclist = list() |
||||
for joint in self.dynamixels: |
||||
if joint.readable: |
||||
synclist.append(joint.id) |
||||
if len(synclist) > 0: |
||||
val = self.device.syncRead(synclist, P_PRESENT_VOLTAGE, 2) |
||||
if val: |
||||
for joint in self.dynamixels: |
||||
try: |
||||
i = synclist.index(joint.id)*2 |
||||
if val[i] < 250: |
||||
joint.voltage = val[i]/10.0 |
||||
if val[i+1] < 100: |
||||
joint.temperature = val[i+1] |
||||
except: |
||||
# not a readable servo |
||||
continue |
||||
else: |
||||
# direct connection, or other hardware with no sync_read capability |
||||
# hack to also use slave_id |
||||
for joint in self.dynamixels: |
||||
if joint.readable: |
||||
val = self.device.read(joint.id, P_PRESENT_VOLTAGE, 2) |
||||
vals = val |
||||
if joint.slave_id > 0: |
||||
vals = self.device.read(joint.slave_id, P_PRESENT_VOLTAGE, 2) |
||||
try: |
||||
joint.voltage = val[0] |
||||
# use maximum |
||||
if val[1] > vals[1]: |
||||
joint.temperature = val[1] |
||||
else: |
||||
joint.temperature = vals[1] |
||||
except: |
||||
continue |
||||
self.iter += 1 |
||||
return None |
||||
""" |
||||
|
||||
@ -0,0 +1,92 @@
@@ -0,0 +1,92 @@
|
||||
#!/usr/bin/env python |
||||
|
||||
""" |
||||
diagnostics.py - diagnostic output code |
||||
Copyright (c) 2011 Vanadium Labs LLC. All right reserved. |
||||
|
||||
Redistribution and use in source and binary forms, with or without |
||||
modification, are permitted provided that the following conditions are met: |
||||
* Redistributions of source code must retain the above copyright |
||||
notice, this list of conditions and the following disclaimer. |
||||
* Redistributions in binary form must reproduce the above copyright |
||||
notice, this list of conditions and the following disclaimer in the |
||||
documentation and/or other materials provided with the distribution. |
||||
* Neither the name of Vanadium Labs LLC nor the names of its |
||||
contributors may be used to endorse or promote products derived |
||||
from this software without specific prior written permission. |
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND |
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
||||
DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
||||
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, |
||||
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF |
||||
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE |
||||
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF |
||||
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
||||
""" |
||||
|
||||
import rospy |
||||
from diagnostic_msgs.msg import DiagnosticArray |
||||
from sensor_msgs.msg import JointState |
||||
|
||||
class DiagnosticsPublisher: |
||||
""" Class to handle publications of joint_states message. """ |
||||
|
||||
def __init__(self): |
||||
self.t_delta = rospy.Duration(1.0/rospy.get_param("~diagnostic_rate", 1.0)) |
||||
self.t_next = rospy.Time.now() + self.t_delta |
||||
self.pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=5) |
||||
|
||||
def update(self, joints, controllers): |
||||
""" Publish diagnostics. """ |
||||
now = rospy.Time.now() |
||||
if now > self.t_next: |
||||
# create message |
||||
msg = DiagnosticArray() |
||||
msg.header.stamp = now |
||||
for controller in controllers: |
||||
d = controller.getDiagnostics() |
||||
if d: |
||||
msg.status.append(d) |
||||
for joint in joints: |
||||
d = joint.getDiagnostics() |
||||
if d: |
||||
msg.status.append(d) |
||||
# publish and update stats |
||||
self.pub.publish(msg) |
||||
self.t_next = now + self.t_delta |
||||
|
||||
|
||||
class JointStatePublisher: |
||||
""" Class to handle publications of joint_states message. """ |
||||
|
||||
def __init__(self): |
||||
# parameters: throttle rate and geometry |
||||
self.rate = rospy.get_param("~read_rate", 10.0) |
||||
self.t_delta = rospy.Duration(1.0/self.rate) |
||||
self.t_next = rospy.Time.now() + self.t_delta |
||||
|
||||
# subscriber |
||||
self.pub = rospy.Publisher('joint_states', JointState, queue_size=5) |
||||
|
||||
def update(self, joints, controllers): |
||||
""" publish joint states. """ |
||||
if rospy.Time.now() > self.t_next: |
||||
msg = JointState() |
||||
msg.header.stamp = rospy.Time.now() |
||||
msg.name = list() |
||||
msg.position = list() |
||||
msg.velocity = list() |
||||
for joint in joints: |
||||
msg.name.append(joint.name) |
||||
msg.position.append(joint.position) |
||||
msg.velocity.append(joint.velocity) |
||||
for controller in controllers: |
||||
msg.name += controller.joint_names |
||||
msg.position += controller.joint_positions |
||||
msg.velocity += controller.joint_velocities |
||||
self.pub.publish(msg) |
||||
self.t_next = rospy.Time.now() + self.t_delta |
||||
|
||||
@ -0,0 +1,49 @@
@@ -0,0 +1,49 @@
|
||||
#!/usr/bin/env python |
||||
|
||||
import sys, time |
||||
import carcomm |
||||
|
||||
if __name__ == "__main__": |
||||
|
||||
try: |
||||
# voorlopig vast |
||||
port = "/dev/ttyUSB0" |
||||
baudrate = 28800 |
||||
parity = 'N' |
||||
rtscts = False |
||||
xonxoff = False |
||||
echo = False |
||||
convert_outgoing = carcomm.CONVERT_CRLF |
||||
repr_mode = 0 |
||||
|
||||
carcon = carcomm.CarControl( |
||||
port, |
||||
baudrate, |
||||
parity, |
||||
rtscts, |
||||
xonxoff, |
||||
echo, |
||||
convert_outgoing, |
||||
repr_mode, |
||||
) |
||||
time.sleep(2) # de nexus robot is niet zo snel.. |
||||
carcon.sendpacket(20,0) |
||||
carcon.getinfopacket() |
||||
|
||||
for x in range(0, 200): |
||||
carcon.setspeed( 2.5, 0.0) |
||||
# carcon.getinfopacket() |
||||
time.sleep(0.05) |
||||
carcon.setspeed( 2.5, 0.0) |
||||
# carcon.getinfopacket() |
||||
time.sleep(0.05) |
||||
carcon.setspeed( 0, 0) |
||||
|
||||
except carcomm.serial.SerialException as e: |
||||
sys.stderr.write("could not open port %r: %s\n" % (port, e)) |
||||
sys.exit(1) |
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,7 @@
@@ -0,0 +1,7 @@
|
||||
# Robotic Fish |
||||
|
||||
A CPG-based locomotion control example of a robotic fish, implemented on the Arduino. |
||||
|
||||
Please visit: |
||||
|
||||
[https://github.com/MartinStokroos/RoFish](https://github.com/MartinStokroos/RoFish) |
||||
Loading…
Reference in new issue