221 lines
8.3 KiB
Python
221 lines
8.3 KiB
Python
#!/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
|
|
"""
|
|
|