#!/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 """