def __init__(self, default_port='/dev/ttyUSB0', default_update_rate=120.0):
"""
@param default_port: default tty port to use for establishing
connection to Eddiebot. This will be overriden by ~port ROS
param if available.
"""
self.default_port = default_port
self.default_update_rate = default_update_rate
self.robot = Eddiebot()
self.sensor_handler = None
self.sensor_state = EddiebotSensorState()
self.req_cmd_vel = None
rospy.init_node('eddiebot')
self._init_params()
self._init_pubsub()
self._pos2d = Pose2D() # 2D pose for odometry
self._diagnostics = EddiebotDiagnostics()
if self.has_gyro:
self._gyro = EddiebotGyro()
else:
self._gyro = None
dynamic_reconfigure.server.Server(EddieBotConfig, self.reconfigure)
self.last_pan_degree = 96 # Default is facing center
self.last_angle = self.UNDEFINED
self.last_dist = self.UNDEFINED
self.emergency_activated = False
self.last_time = 0
self.cmd_log = open(
'/home/eddie/logs/cmd_log_' + str(int(time.time())), 'w')
self.cmd_log.write(
'cur_time,msg.linear.x,msg.angular.z,linear_speed_ticks,angular_speed_ticks,left_spd,right_spd\n'
)
from decimal import *
logging.basicConfig(filename='test_eddiebot_driver.log', level=logging.INFO)
# configure the serial connections (the parameters differs on the device you are connecting to)
ser = serial.Serial(port='/dev/ttyUSB0',
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS)
ser.open()
ser.isOpen()
print 'Enter your commands below.\r\nInsert "exit" to leave the application.'
eddiebot = Eddiebot()
input = 1
while 1:
# get keyboard input
command = raw_input("command: ")
if command == "": # Stop the Eddie robot
# send the character to the device
# (note that I happend a \r\n carriage return and line feed to the characters - this is requested by my device)
ser.write('STOP' + ' 0' + chr(13))
out = ''
# let's wait one second before reading output (let's give device time to answer)
time.sleep(1)
while ser.inWaiting() > 0:
out += ser.read(1)
