class AppCommunication(object):
def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
self.SkeletonMessage = '0:'
self.HandMessage = '0:'
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudRate = int(rospy.get_param("~baudRate", "115200"))
self.Serial = SerialDataGateway(port, baudrate, self.ApplicationReceiving)
def SendMsg(self, data):
msgout = ApplicationMessage()
msgout.Type = ApplicationMessage.APP_MSG
msgout.Data = data
self.Serial.write(msgout.ToString())
def StartApp(self):
rospy.loginfo("Starting serial gateway")
time.sleep(.1)
self.Serial.Start()
def StopApp(self):
rospy.loginfo("Stopping serial gateway")
time.sleep(.1)
self.Serial.Stop()
def ApplicationReceiving(self, stream):
msgin = ApplicationMessage()
msgin.ParseString(stream)
msgout = ApplicationMessage()
if msgin.Type == ApplicationMessage.APP_SKELETON:
#Skeleton msg
self.SkeletonMessage = msgin.Data
rospy.loginfo(msgin.Data)
return
elif msgin.Type == ApplicationMessage.APP_HAND:
#Hand msg
self.HandMessage = msgin.Data
rospy.loginfo(msgin.Data)
return
def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
self.SkeletonMessage = '0:'
self.HandMessage = '0:'
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudRate = int(rospy.get_param("~baudRate", "115200"))
self.Serial = SerialDataGateway(port, baudrate, self.ApplicationReceiving)
class Launchpad_Class(object):
def __init__(self):
print "Initializing Launchpad Class"
#######################################################################################################################
# Sensor variables
self._Counter = 0
self._dxy = 0
self._dth = 0
self._left_encoder_value = 0
self._right_encoder_value = 0
self._battery_value = 0
self._ultrasonic_value = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
self._left_wheel_speed_ = 0
self._right_wheel_speed_ = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
#######################################################################################################################
# Get serial port and baud rate of Tiva C Launchpad
port = rospy.get_param("~port", "/dev/ttyS0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
# Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
self.dxy = rospy.Publisher('dxy', Float32, queue_size=10)
self.dth = rospy.Publisher('dth', Float32, queue_size=10)
# Publisher for left and right wheel encoder values
self._Left_Encoder = rospy.Publisher('lwheel', Int64, queue_size=10)
self._Right_Encoder = rospy.Publisher('rwheel', Int64, queue_size=10)
# Publisher for Battery level(for upgrade purpose)
self._Battery_Level = rospy.Publisher('battery_level',
Float32,
queue_size=10)
# Publisher for Ultrasonic distance sensor
self._Ultrasonic_Value = rospy.Publisher('ultrasonic_distance',
Float32,
queue_size=10)
# Publisher for IMU rotation quaternion values
#self._qx_ = rospy.Publisher('qx', Float32, queue_size=10)
#self._qy_ = rospy.Publisher('qy', Float32, queue_size=10)
#self._qz_ = rospy.Publisher('qz', Float32, queue_size=10)
#self._qw_ = rospy.Publisher('qw', Float32, queue_size=10)
# Publisher for entire serial data
#self._SerialPublisher = rospy.Publisher(
# 'serial', String, queue_size=10)
#######################################################################################################################
# Subscribers and Publishers of IMU data topic
self.frame_id = '/base_footprint'
self.reach = 0
self.cal_offset = 0.0
self.orientation = 0.0
self.cal_buffer = []
self.cal_buffer_length = 1000
self.imu_data = Imu(header=rospy.Header(frame_id="base_footprint"))
self.imu_data.orientation_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.angular_velocity_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.linear_acceleration_covariance = [
-1, 0, 0, 0, 0, 0, 0, 0, 0
]
self.gyro_measurement_range = 150.0
self.gyro_scale_correction = 1.35
#self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.deltat = 0
self.lastUpdate = 0
# New addon for computing quaternion
self.pi = 3.14159
self.GyroMeasError = float(self.pi * (40 / 180))
self.beta = float(math.sqrt(3 / 4) * self.GyroMeasError)
self.GyroMeasDrift = float(self.pi * (2 / 180))
self.zeta = float(math.sqrt(3 / 4) * self.GyroMeasDrift)
self.beta = math.sqrt(3 / 4) * self.GyroMeasError
self.q = [1, 0, 0, 0]
#######################################################################################################################
# Speed subscriber
#self._left_motor_speed = rospy.Subscriber(
# 'left_wheel_speed', Float32, self._Update_Left_Speed)
#self._right_motor_speed = rospy.Subscriber(
# 'right_wheel_speed', Float32, self._Update_Right_Speed)
#rospy.Subscriber('cmd_vel', Twist, self._handle_cmd_vel)
self.sub = rospy.Subscriber('/move_base/DWAPlannerROS/global_plan',
Path, self.clbk_path)
rospy.Subscriber('/odom', Odometry, self.clbk_odom)
rospy.Timer(rospy.Duration(0.1), self.timerCB)
rospy.Subscriber('/pure', String, self.clbk_pure)
self.odom = Odometry()
def clbk_pure(self, msg):
self._WriteSerial(msg.data)
def timerCB(self, event):
odom_msg = "CDN"
odom_msg += "," + str(int(self.odom.pose.pose.position.x * 1000))
odom_msg += "," + str(int(self.odom.pose.pose.position.y * 1000))
ori_li = [
self.odom.pose.pose.orientation.x,
self.odom.pose.pose.orientation.y,
self.odom.pose.pose.orientation.z,
self.odom.pose.pose.orientation.w
]
(roll, pitch, yaw) = euler_from_quaternion(ori_li)
odom_msg += "," + str(int(yaw * 10)) + '\n'
self._WriteSerial(odom_msg)
if (self.reach == 1):
self.reach = 0
self.sub = rospy.Subscriber('/move_base/DWAPlannerROS/global_plan',
Path, self.clbk_path)
def clbk_odom(self, msg):
self.odom.pose.pose.position.x = msg.pose.pose.position.x
self.odom.pose.pose.position.y = msg.pose.pose.position.y
self.odom.pose.pose.position.z = msg.pose.pose.position.z
self.odom.pose.pose.orientation.x = msg.pose.pose.orientation.x
self.odom.pose.pose.orientation.y = msg.pose.pose.orientation.y
self.odom.pose.pose.orientation.z = msg.pose.pose.orientation.z
self.odom.pose.pose.orientation.w = msg.pose.pose.orientation.w
def clbk_path(self, msg):
size_msg = "PAS," + str(len(msg.poses)) + '\n'
self._WriteSerial(size_msg)
poi_msg = "POI"
#pay_msg = "PAY"
#paa_msg = "PAA"
for pose in msg.poses:
ori_q = pose.pose.orientation
ori_list = [ori_q.x, ori_q.y, ori_q.z, ori_q.w]
(roll, pitch, yaw) = euler_from_quaternion(ori_list)
#path_msg = "CDN,"+str(int(pose.pose.position.x*1000))+","+str(int(pose.pose.position.y*1000))+","+str(int(yaw*10))+'\n'
#self._WriteSerial(path_msg)
poi_msg += "," + str(int(pose.pose.position.x * 1000))
poi_msg += "," + str(int(pose.pose.position.y * 1000))
#paa_msg += ","+str(int(yaw * 10))
poi_msg += '\n'
#pay_msg += '\n'
#paa_msg += '\n'
#time.sleep(2)
self._WriteSerial(poi_msg)
#time.sleep(1)
#self._WriteSerial(pay_msg)
#self.sub.unregister()
#time.sleep(1)
#self._WriteSerial(paa_msg)
#time.sleep(1)
def _handle_cmd_vel(self, msg):
cmd_message = "VES," + str(int(msg.linear.x * 1000)) + '\n'
self._WriteSerial(cmd_message)
cmd_message = "WHS," + str(int(msg.angular.z * 10)) + '\n'
self._WriteSerial(cmd_message)
#######################################################################################################################
def _Update_Left_Speed(self, left_speed):
self._left_wheel_speed_ = left_speed.data
rospy.loginfo(left_speed.data)
speed_message = 's %d %d\r' % (int(
self._left_wheel_speed_), int(self._right_wheel_speed_))
self._WriteSerial(speed_message)
# 3
def _Update_Right_Speed(self, right_speed):
self._right_wheel_speed_ = right_speed.data
rospy.loginfo(right_speed.data)
speed_message = 's %d %d\r' % (int(
self._left_wheel_speed_), int(self._right_wheel_speed_))
self._WriteSerial(speed_message)
#######################################################################################################################
# Calculate orientation from accelerometer and gyrometer
def _HandleReceivedLine(self, line):
#rospy.loginfo(line)
#self._Counter = self._Counter + 1
#self._SerialPublisher.publish(
# String(str(self._Counter) + ", in: " + line))
if (len(line) > 0):
lineParts = line.split(',')
try:
if (lineParts[0] == 'DXY'):
self._dxy = float(lineParts[1])
rospy.loginfo("DXY:" + str(self._dxy))
self.dxy.publish(self._dxy)
if (lineParts[0] == 'DTH'):
self._dth = float(lineParts[1])
rospy.loginfo("DTH:" + str(self._dth))
self.dth.publish(self._dth)
if (lineParts[0] == 'REA'):
self.reach = 1
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
#######################################################################################################################
def _WriteSerial(self, message):
# self._SerialPublisher.publish(
# String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
#######################################################################################################################
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
#######################################################################################################################
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
#######################################################################################################################
def Subscribe_Speed(self):
a = 1
# print "Subscribe speed"
#######################################################################################################################
def Reset_Launchpad(self):
print "Reset"
reset = 'r\r'
self._WriteSerial(reset)
time.sleep(1)
self._WriteSerial(reset)
time.sleep(2)
#######################################################################################################################
def Send_Speed(self):
# print "Set speed"
a = 3
class Arduino(object):
'''
Helper class for communicating with an Arduino board over serial port
'''
CONTROLLER_RESET_REQUIRED = 0
CONTROLLER_INITIALIZING = 1
CONTROLLER_IS_READY = 2
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
#rospy.logdebug(str(self._Counter) + " " + line)
#if (self._Counter % 50 == 0):
self._SerialPublisher.publish(String(str(self._Counter) + " " + line))
if (len(line) > 0):
lineParts = line.split('\t')
if (lineParts[0] == 'o'):
self._BroadcastOdometryInfo(lineParts)
return
if (lineParts[0] == 'b'):
self._BroadcastBatteryInfo(lineParts)
return
if (lineParts[0] == 'm'):
self._Broadcastright(lineParts)
return
if (lineParts[0] == 'n'):
self._Broadcastleft(lineParts)
return
if (lineParts[0] == 'd'):
self._BroadcastAuto(lineParts)
return
if (lineParts[0] == "InitializeDriveGeometry"):
# controller requesting initialization
self._InitializeDriveGeometry()
return
if (lineParts[0] == "InitializeSpeedController"):
# controller requesting initialization
self._InitializeSpeedController()
return
if (lineParts[0] == "InitializeBatteryMonitor"):
# controller requesting initialization
self._InitializeBatteryMonitor()
return
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
theta = float(lineParts[3])
vx = float(lineParts[4])
omega = float(lineParts[5])
#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_footprint over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_footprint", "odom")
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_footprint"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
#rospy.loginfo(odometry)
except:
rospy.logwarn("Unexpected error odomfrom arduino.py :" +
str(sys.exc_info()[0]))
def _BroadcastBatteryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 1):
pass
try:
self.batteryVoltage = float(lineParts[1])
#batteryCurrent = float(lineParts[2])
#batteryState = Float32()
#batteryState.voltage = batteryVoltage
#batteryState.current = batteryCurrent
self._BatteryStatePublisher.publish(self.batteryVoltage)
rospy.loginfo(batteryVoltage)
except:
pass
#rospy.logwarn("Unexpected error battery:" + str(sys.exc_info()[0]))
def _Broadcastleft(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 1):
pass
try:
V_left = float(lineParts[1])
self._V_leftPublisher.publish(V_left)
rospy.loginfo(V_left)
except:
pass
#rospy.logwarn("Unexpected error V_left:" + str(sys.exc_info()[0]))
def _Broadcastright(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 1):
pass
try:
V_right = float(lineParts[1])
self._V_rightPublisher.publish(V_right)
rospy.loginfo(V_right)
except:
pass
rospy.logwarn("Unexpected error V_right:" + str(sys.exc_info()[0]))
def _BroadcastAuto(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 1):
pass
try:
self.auto_left = int(lineParts[1])
self.auto_right = int(lineParts[2])
self.auto_bumper = int(lineParts[3])
self.docked = abs(self.auto_bumper - 1)
self.charged = round((self.docked * self.batteryVoltage), 2)
self._dock_state_Publisher.publish(self.charged)
except:
#pass
rospy.logwarn("Unexpected error auto dock:" +
str(sys.exc_info()[0]))
def _WriteSerial(self, message):
self._SerialPublisher.publish(
String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
def __init__(self, port="/dev/sensors/ftdi_A500CNMH", baudrate=57600):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
self._Counter = 0
rospy.init_node('arduino')
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.logwarn("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
rospy.Subscriber("auto_dock", String, self._AutoDock)
# A service to maually start Auto dock
rospy.Service('start_auto_dock', AutoDock, self.Start_Auto_Dock)
self._SerialPublisher = rospy.Publisher('serial', String, queue_size=5)
self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
self._OdometryPublisher = rospy.Publisher("odom",
Odometry,
queue_size=5)
self._VoltageLowlimit = rospy.get_param(
"~batteryStateParams/voltageLowlimit", "12.0")
self._VoltageLowLowlimit = rospy.get_param(
"~batteryStateParams/voltageLowLowlimit", "11.7")
self._BatteryStatePublisher = rospy.Publisher("battery_level",
Float32,
queue_size=5)
self._V_leftPublisher = rospy.Publisher("v_left",
Float32,
queue_size=5)
self._V_rightPublisher = rospy.Publisher("v_right",
Float32,
queue_size=5)
self._dock_state_Publisher = rospy.Publisher("charge_state",
Float32,
queue_size=5)
#self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)
self._State = Arduino.CONTROLLER_RESET_REQUIRED
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
def _HandleVelocityCommand(self, twistCommand):
""" Handle movement requests. """
#v = twistCommand.linear.x # m/s
#$omega = twistCommand.angular.z # rad/s
x = twistCommand.linear.x # m/s
th = twistCommand.angular.z # rad/s
if x == 0:
# Turn in place
right = th * 0.43 / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * 0.43 / 2.0
right = x + th * 0.43 / 2.0
v_des_left = int(left * 16065 / 30) #
v_des_right = int(right * 16028 / 30) #ticks_per_meter 15915
#rospy.logwarn("Handling twist ommand: " + str(v_des_left) + "," + str(v_des_right))
#message = 's %.2f %.2f\r' % (v_des_left, v_des_right)
message = 's %d %d %d %d \r' % self._GetBaseAndExponents(
(v_des_left, v_des_right))
#rospy.logwarn("Sending speed command message: " + message)
#self._WriteSerial(message)
self._SerialDataGateway.Write(message)
def _InitializeDriveGeometry(self):
wheelDiameter = rospy.get_param("~driveGeometry/wheelDiameter", "0")
trackWidth = rospy.get_param("~driveGeometry/trackWidth", "0")
countsPerRevolution = rospy.get_param(
"~driveGeometry/countsPerRevolution", "0")
#wheelDiameterParts = self._GetBaseAndExponent(wheelDiameter)
#trackWidthParts = self._GetBaseAndExponent(trackWidth)
message = 'dg %f %f %d\r' % (wheelDiameter, trackWidth,
countsPerRevolution)
rospy.logdebug("Sending drive geometry params message: " + message)
self._WriteSerial(message)
def _InitializeSpeedController(self):
velocityPParam = rospy.get_param("~speedController/velocityPParam",
"0")
velocityIParam = rospy.get_param("~speedController/velocityIParam",
"0")
turnPParam = rospy.get_param("~speedController/turnPParam", "0")
turnIParam = rospy.get_param("~speedController/turnIParam", "0")
commandTimeout = self._GetCommandTimeoutForSpeedController()
message = 'sc %.2f %.2f %.2f %.2f %.2f\r' % (
velocityPParam, velocityIParam, turnPParam, turnIParam,
commandTimeout)
#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
rospy.logdebug("Sending differential drive gains message: " + message)
self._WriteSerial(message)
#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
#rospy.loginfo(str(speedControllerParams))
#self._WriteSpeedControllerParams(speedControllerParams)
def _GetCommandTimeoutForSpeedController(self):
"""
Returns the command timeout for the speed controller in seconds.
If no velocity command arrives for more than the specified timeout then the speed controller will stop the robot.
"""
return rospy.get_param("~speedController/commandTimeout", "0.5")
def _HandleSetDriveGains(self, request):
""" Handle the setting of the drive gains (PID). """
# We persist the new values in the parameter server
rospy.set_param(
"~speedController", {
'velocityPParam': request.velocityPParam,
'velocityPParam': request.velocityIParam,
'turnPParam': request.turnPParam,
'turnIParam': request.turnIParam
})
commandTimeout = self._GetCommandTimeoutForSpeedController()
speedControllerParams = (request.velocityPParam,
request.velocityIParam, request.turnPParam,
request.turnIParam, commandTimeout)
self._WriteSpeedControllerParams(speedControllerParams)
return SetDriveControlGainsResponse()
def _WriteSpeedControllerParams(self, speedControllerParams):
""" Writes the speed controller parameters (drive gains (PID), and command timeout) to the Arduino controller. """
rospy.logdebug(
"Handling '_WriteSpeedControllerParams'; received parameters " +
str(speedControllerParams))
message = 'SpeedCo %d %d %d %d %d %d %d %d %d %d\r' % self._GetBaseAndExponents(
speedControllerParams)
message = 'SpeedCo 763 -4 3700 -4 9750\r'
rospy.logdebug("Sending differential drive gains message: " + message)
self._WriteSerial(message)
def _InitializeBatteryMonitor(self):
rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " +
str(self._VoltageLowLowlimit))
message = 'bm %f\r' % self._VoltageLowLowlimit
rospy.logdebug("Sending battery monitor params message: " + message)
self._WriteSerial(message)
def _AutoDock(self, data):
'''
defunct remove after test
'''
x = data.data
if x == "dock":
message = 'd 1\r'
else:
message = 'd 0\r'
rospy.logwarn("Sending Auto Docks message: " + message)
self._WriteSerial(message)
def Start_Auto_Dock(self, data):
'''
start auto dock and charging sequance on the arduino
it will release when full
'''
x = data.data
print x
if x == "dock":
message = 'd 1\r'
else:
message = 'd 0\r'
rospy.logwarn("Sending Auto Docks message: " + message)
self._WriteSerial(message)
if self.charged > 13.5:
return AutoDockResponse()
def _GetBaseAndExponent(self, floatValue, resolution=4):
'''
Converts a float into a tuple holding two integers:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
if (floatValue == 0.0):
return (0, 0)
else:
exponent = int(1.0 + math.log10(abs(floatValue)))
multiplier = math.pow(10, resolution - exponent)
base = int(floatValue * multiplier)
return (base, exponent - resolution)
def _GetBaseAndExponents(self, floatValues, resolution=4):
'''
Converts a list or tuple of floats into a tuple holding two integers for each float:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
baseAndExponents = []
for floatValue in floatValues:
baseAndExponent = self._GetBaseAndExponent(floatValue)
baseAndExponents.append(baseAndExponent[0])
baseAndExponents.append(baseAndExponent[1])
return tuple(baseAndExponents)
class Arduino(object):
'''
Helper class for communicating with an Arduino board over serial port
'''
CONTROLLER_RESET_REQUIRED = 0
CONTROLLER_INITIALIZING = 1
CONTROLLER_IS_READY = 2
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
#rospy.logdebug(str(self._Counter) + " " + line)
#if (self._Counter % 50 == 0):
self._SerialPublisher.publish(
String(str(self._Counter) + ", in: " + line))
if (len(line) > 0):
lineParts = line.split('\t')
#if (self._State == Arduino.CONTROLLER_RESET_REQUIRED):
#if (lineParts[0] == "reset_done"):
#self._State = Arduino.CONTROLLER_INITIALIZING
#return
#else:
#self._WriteSerial('reset\r')
#return
if (lineParts[0] == 'o'):
self._BroadcastOdometryInfo(lineParts)
return
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
theta = float(lineParts[3])
vx = float(lineParts[4])
vy = float(lineParts[1])
omega = float(lineParts[1])
self.batteryVoltage = float(lineParts[15]) * 0.0287
#BatteryState = BatteryState()
#BatteryState = batteryVoltage * 0.287
# calculate position
#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_link", "odom")
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.pose.covariance = [
1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0,
0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3
]
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = vy
odometry.twist.twist.angular.z = omega
odometry.twist.covariance = [
1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0,
0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3
]
self._OdometryPublisher.publish(odometry)
#self._BatteryStatePublisher.publish(12)
#rospy.loginfo(self.batteryVoltage)
except:
rospy.logwarn("odom Unexpected error:" + str(sys.exc_info()[0]))
def _WriteSerial(self, message):
self._SerialPublisher.publish(
String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
self._Counter = 0
rospy.init_node('arduino')
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
#BatteryState = BatteryState()
#BatteryState.voltage = self.batteryVoltage
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
self._SerialPublisher = rospy.Publisher('serial', String)
self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
self._OdometryPublisher = rospy.Publisher("odom", Odometry)
#self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
#self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")
#self._BatteryStatePublisher = rospy.Publisher("batteryVolt", BatteryState)
self._State = Arduino.CONTROLLER_RESET_REQUIRED
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
def Start(self):
rospy.loginfo("Starting serial gateway")
self._SerialDataGateway.Start()
message = 'r \r'
self._WriteSerial(message)
def Stop(self):
rospy.loginfo("Stopping")
message = 'r \r'
self._WriteSerial(message)
sleep(5)
self._SerialDataGateway.Stop()
def _BroadcastBatteryInfo(self, lineParts):
#partsCount = len(lineParts)
#rospy.logwarn(partsCount)
pass
def _HandleVelocityCommand(self, twistCommand):
""" Handle movement requests. send wheel RPM over 10 mins"""
self.wheel_track = .26
self.gear_reduction = 1
self.ticks_per_meter = .484 #meters per revulotion
x = twistCommand.linear.x # m/s
th = twistCommand.angular.z # rad/s
if x == 0:
# Turn in place
right = th * self.wheel_track * self.gear_reduction / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * self.wheel_track * self.gear_reduction / 2.0
right = x + th * self.wheel_track * self.gear_reduction / 2.0
Left = int(left / self.ticks_per_meter * 600)
Right = int(right / self.ticks_per_meter * 600)
message = 's %d %d %d %d \r' % self._GetBaseAndExponents(
(Left, Right))
rospy.loginfo("Sending speed command message: " + message)
self._WriteSerial(message)
def _GetBaseAndExponent(self, floatValue, resolution=4):
'''
Converts a float into a tuple holding two integers:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
if (floatValue == 0.0):
return (0, 0)
else:
exponent = int(1.0 + math.log10(abs(floatValue)))
multiplier = math.pow(10, resolution - exponent)
base = int(floatValue * multiplier)
return (base, exponent - resolution)
def _GetBaseAndExponents(self, floatValues, resolution=4):
'''
Converts a list or tuple of floats into a tuple holding two integers for each float:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
baseAndExponents = []
for floatValue in floatValues:
baseAndExponent = self._GetBaseAndExponent(floatValue)
baseAndExponents.append(baseAndExponent[0])
baseAndExponents.append(baseAndExponent[1])
return tuple(baseAndExponents)
class Robot(object):
def __init__(self):
rospy.init_node('robot_setup')
self.tfBroadcaster = tf.TransformBroadcaster()
self.odomPublisher = rospy.Publisher('odom', Odometry, queue_size=10)
self.laserScanPublisher = rospy.Publisher('laserScan',
LaserScan,
queue_size=10)
port = rospy.get_param("~port", "/dev/ttyUSB0")
baud_rate = int(rospy.get_param("~baudRate", 115200))
self.robotSerial = SerialDataGateway(port, baud_rate,
self._handle_received_line)
rospy.Subscriber("/cmd_vel", Twist, self.receive_cmd_vel)
self.x, self.y, self.yaw, self.v, self.w = 0.0, 0.0, 0.0, 0.0, 0.0 # start point
self.cmd = "vel 0.0 0.0"
self.temp_v, self.temp_w, self.trigger = 0.0, 0.0, False
#------------------------------handle serial command--------------------------
def _handle_received_line(self, data):
# ['x:1.00', 'y:2.00', 'heading:3.00', 'omega:55.00', 'v:66.00']
try:
robotState = data.split(',')
robotState = [item[item.index(':') + 1:] for item in robotState]
robotState = map(float, robotState)
self.x, self.y, self.yaw, self.w, self.v = robotState
rospy.loginfo("Received x:%.2f y:%.2f theta:%.2f v:%.2f w:%.2f" %
(self.x, self.y, self.yaw, self.v, self.w))
except:
#rospy.loginfo("Received error wrong data type %s"%data)
pass
def receive_cmd_vel(self, data, b=0.8, trigger=True):
v, w = data.linear.x, data.angular.z
if (self.temp_v == v and self.temp_w == w):
self.trigger = False
else:
self.trigger = True
self.temp_v, self.temp_w = v, w
self.cmd = "vel %.1f %.1f\r" % (v, w) # v(0~0.5) w(0~?)
# cvt center v and w to vl and vr
# vr = v + b/2*w
# vl = v - b/2*w
#-------------------------------------------------------------------------------
def broadcastTF(self, data=None):
x, y, theta = self.x, self.y, self.yaw
# translation, orientation, time, child, parent
self.tfBroadcaster.sendTransform((0, 0, 0),
quaternion_from_euler(0, 0, 0),
rospy.Time.now(), "odom", "/map")
self.tfBroadcaster.sendTransform((x, y, 0),
quaternion_from_euler(0, 0, theta),
rospy.Time.now(), "base_link", "odom")
self.tfBroadcaster.sendTransform((0.0, 0.0, 0.5),
quaternion_from_euler(0, 0, 0),
rospy.Time.now(), "base_laser",
"base_link")
def publishOdometry(self, data=None):
odom = Odometry()
odom.header.seq = 0
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
q = quaternion_from_euler(0.0, 0.0, self.yaw)
odom.pose.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
odom.twist.twist.linear.x = self.v
odom.twist.twist.angular.z = self.w
self.odomPublisher.publish(odom)
def publishLaserScan(self, data=None):
scan = LaserScan()
scan.header.seq = 1
scan.header.stamp = rospy.Time.now()
num_readings = 100
laser_frequency = 40
scan.header.frame_id = "base_laser"
scan.angle_min = radians(-30)
scan.angle_max = radians(30)
scan.angle_increment = radians(60) / num_readings
scan.time_increment = (1 / laser_frequency) / (num_readings)
scan.range_min = 0.5
scan.range_max = 6
scan.ranges = [5] * num_readings
self.laserScanPublisher.publish(scan)
def publishCmd(self):
if temp == self.cmd:
pass
else:
self.robotSerial.Write(self.cmd)
temp = self.cmd
def logInfo(self):
#rospy.loginfo("x:%.2f y:%.2f theta:%.2f v:%.2f w:%.2f"%(self.x,self.y,self.yaw,self.v,self.w))
#rospy.loginfo(self.cmd)
pass
def start(self):
rate = rospy.Rate(10.0)
rospy.loginfo("Robot Engine Start")
self.robotSerial.Start()
while not rospy.is_shutdown():
try:
self.broadcastTF()
self.publishOdometry()
self.publishLaserScan()
self.logInfo()
rate.sleep()
finally:
self.robotSerial.Stop()
def testScript(self):
rate = rospy.Rate(10.0)
rospy.loginfo("Robot Engine Start")
self.robotSerial.Start()
self.robotSerial.Write("start\r")
try:
while not rospy.is_shutdown():
if self.trigger:
self.robotSerial.Write(self.cmd)
rospy.loginfo(self.cmd)
self.broadcastTF()
self.publishOdometry()
#self.publishLaserScan()
#self.logInfo()
rate.sleep()
finally:
self.robotSerial.Write("stop\r")
self.robotSerial.Stop()
rospy.loginfo("Have a good day bye-bye")
class Launchpad_Class(object):
def __init__(self):
print "Iniciando clase launchpad"
######################################
#Variables de los sensores
self._Counter = 0
self._reset_flipper1_value = 0
self._reset_flipper2_value = 0
self._reset_flipper3_value = 0
self._reset_flipper4_value = 0
self._left_encoder_value = 0
self._left_wheel_speed = 0
self._right_encoder_value = 0
self._right_wheel_speed = 0
self._flipper1_encoder_value = 0
self._flipper1_wheel_speed = 0
self._flipper2_encoder_value = 0
self._flipper2_wheel_speed = 0
self._flipper3_encoder_value = 0
self._flipper3_wheel_speed = 0
self._flipper4_encoder_value = 0
self._flipper4_wheel_speed = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
#########################################
#Asignamos valores del puerto y baudios de la stellaris
# port=rospy.get_param("~port","/dev/ttyACM0")
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#########################################
rospy.loginfo("starting with serialport:" + port + "baudrate:" +
str(baudRate))
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
rospy.loginfo("started serial communication")
###########################################
#publisher y suscribers
self._Left_Encoder = rospy.Publisher('left_lec', Int16, queue_size=10)
self._SerialPublisher = rospy.Publisher('serial_base',
String,
queue_size=10)
self.deltat = 0
self.lastUpdate = 0
self._Right_Encoder = rospy.Publisher('right_lec',
Int16,
queue_size=10)
##################################################################
##Publisher for flipper reset
self._Flipper1_Reset = rospy.Publisher('flipper1_reset',
Int16,
queue_size=10)
self._Flipper2_Reset = rospy.Publisher('flipper2_reset',
Int16,
queue_size=10)
self._Flipper3_Reset = rospy.Publisher('flipper3_reset',
Int16,
queue_size=10)
self._Flipper4_Reset = rospy.Publisher('flipper4_reset',
Int16,
queue_size=10)
##################################################################
##Publisher for encoders
self._Flipper1_Encoder = rospy.Publisher('flip1_lec',
Int16,
queue_size=10)
self._Flipper2_Encoder = rospy.Publisher('flip2_lec',
Int16,
queue_size=10)
self._Flipper3_Encoder = rospy.Publisher('flip3_lec',
Int16,
queue_size=10)
self._Flipper4_Encoder = rospy.Publisher('flip4_lec',
Int16,
queue_size=10)
##################################################################
##Publisher for IMU rotation quaternion values
self._qx = rospy.Publisher('qx', Float32, queue_size=10)
self._qy = rospy.Publisher('qy', Float32, queue_size=10)
self._qz = rospy.Publisher('qz', Float32, queue_size=10)
self._qw = rospy.Publisher('qw', Float32, queue_size=10)
self.frame_id = '/base_link'
self.cal_offset = 0.0
self.orientation = 0.0
self.cal_buffer = []
self.cal_buffer_length = 1000
self.imu_data = Imu(header=rospy.Header(frame_id="base_link"))
self.imu_data.orientation_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.angular_velocity_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.linear_acceleration_covariance = [
-1, 0, 0, 0, 0, 0, 0, 0, 0
]
self.gyro_measurement_range = 150.0
self.gyro_scale_correction = 1.35
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.pi = 3.1416
self._left_motor_speed = rospy.Subscriber('left_out', Int16,
self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_out', Int16,
self._Update_Right_Speed)
self._flipper1_speed = rospy.Subscriber('flipper1_out', Int16,
self._Update_Flipper1_Speed)
self._flipper2_speed = rospy.Subscriber('flipper2_out', Int16,
self._Update_Flipper2_Speed)
self._flipper3_speed = rospy.Subscriber('flipper3_out', Int16,
self._Update_Flipper3_Speed)
self._flipper4_speed = rospy.Subscriber('flipper4_out', Int16,
self._Update_Flipper4_Speed)
def _Update_Left_Speed(self, left_speed):
self._left_wheel_speed = left_speed.data
#rospy.loginfo(left_speed.data)
speed_message = 's %d %d \r' % (int(
self._left_wheel_speed), int(self._right_wheel_speed))
self._WriteSerial(speed_message)
def _Update_Right_Speed(self, right_speed):
self._right_wheel_speed = right_speed.data
#rospy.loginfo(right_speed.data)
speed_message = 's %d %d \r' % (int(
self._left_wheel_speed), int(self._right_wheel_speed))
self._WriteSerial(speed_message)
def _Update_Flipper1_Speed(self, flipper1_speed):
self._flipper1_wheel_speed = flipper1_speed.data
#rospy.loginfo(flipper1_speed.data)
speed_message = 'f %d %d %d %d\r' % (
int(self._flipper1_wheel_speed), int(self._flipper2_wheel_speed),
int(self._flipper3_wheel_speed), int(self._flipper4_wheel_speed))
self._WriteSerial(speed_message)
def _Update_Flipper2_Speed(self, flipper2_speed):
self._flipper2_wheel_speed = flipper2_speed.data
#rospy.loginfo(flipper2_speed.data)
speed_message = 'f %d %d %d %d\r' % (
int(self._flipper1_wheel_speed), int(self._flipper2_wheel_speed),
int(self._flipper3_wheel_speed), int(self._flipper4_wheel_speed))
self._WriteSerial(speed_message)
def _Update_Flipper3_Speed(self, flipper3_speed):
self._flipper3_wheel_speed = flipper3_speed.data
#rospy.loginfo(flipper3_speed.data)
speed_message = 'f %d %d %d %d\r' % (
int(self._flipper1_wheel_speed), int(self._flipper2_wheel_speed),
int(self._flipper3_wheel_speed), int(self._flipper4_wheel_speed))
self._WriteSerial(speed_message)
def _Update_Flipper4_Speed(self, flipper4_speed):
self._flipper4_wheel_speed = flipper4_speed.data
#rospy.loginfo(flipper4_speed.data)
speed_message = 'f %d %d %d %d\r' % (
int(self._flipper1_wheel_speed), int(self._flipper2_wheel_speed),
int(self._flipper3_wheel_speed), int(self._flipper4_wheel_speed))
self._WriteSerial(speed_message)
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(
String(str(self._Counter) + ", in:" + line))
if (len(line) > 0):
lineParts = line.split('\t')
try:
if (lineParts[0] == 'e'):
self._left_encoder_value = long(lineParts[1])
self._right_encoder_value = long(lineParts[2])
self._flipper1_encoder_value = long(lineParts[3])
self._flipper2_encoder_value = long(lineParts[4])
self._flipper3_encoder_value = long(lineParts[5])
self._flipper4_encoder_value = long(lineParts[6])
self._Left_Encoder.publish(self._left_encoder_value)
self._Right_Encoder.publish(self._right_encoder_value)
self._Flipper1_Encoder.publish(
self._flipper1_encoder_value)
self._Flipper2_Encoder.publish(
self._flipper2_encoder_value)
self._Flipper3_Encoder.publish(
self._flipper3_encoder_value)
self._Flipper4_Encoder.publish(
self._flipper4_encoder_value)
if (lineParts[0] == 'n'):
self.reset_flipper1_value = int(lineParts[1])
self.reset_flipper2_value = int(lineParts[2])
self.reset_flipper3_value = int(lineParts[3])
self.reset_flipper4_value = int(lineParts[4])
self._Flipper1_Reset.publish(self.reset_flipper1_value)
self._Flipper2_Reset.publish(self.reset_flipper2_value)
self._Flipper3_Reset.publish(self.reset_flipper3_value)
self._Flipper4_Reset.publish(self.reset_flipper4_value)
except:
rospy.logwarn("Error in sensor values")
rospy.logwarn(lineParts)
pass
def _WriteSerial(self, message):
self._SerialPublisher.publish(
String(str(self._Counter) + ", out:" + message))
self._SerialDataGateway.Write(message)
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
def Stop(self):
rospy.logdebug("Stoping")
self._SerialDataGateway.Stop()
def Reset_Launchpad(self):
print "reset"
reset = 'r\r'
self._WriteSerial(reset)
time.sleep(1)
self._WriteSerial(reset)
time.sleep(2)
def SubscribeSpeed(self):
a = 1
def SendSpeed(self):
a = 3
class PropellerComm(object):
"""
Helper class for communicating with a Propeller board over serial port
"""
def __init__(self):
rospy.init_node('arlobot')
self.r = rospy.Rate(1) # 1hz refresh rate
self._Counter = 0 # For Propeller code's _HandleReceivedLine and _write_serial
self._motorsOn = False # Set to 1 if the motors are on, used with USB Relay Control board
self._SafeToOperate = False # Use arlobot_safety to set this
self._acPower = False # Track AC power status internally
self._unPlugging = False # Used for when arlobot_safety tells us to "UnPlug"!
self._wasUnplugging = False # Track previous unplugging status for motor control
self._SwitchingMotors = False # Prevent overlapping calls to _switch_motors
self._serialAvailable = False
self._serialTimeout = 0
self._leftMotorPower = False
self._rightMotorPower = False
self._laptop_battery_percent = 100
# Store last x, y and heading for reuse when we reset
# I took off off the ~, because that was causing these to reset to default on every restart
# even if roscore was still up.
self.lastX = rospy.get_param("lastX", 0.0)
self.lastY = rospy.get_param("lastY", 0.0)
self.lastHeading = rospy.get_param("lastHeading", 0.0)
self.alternate_heading = self.lastHeading
self.track_width = rospy.get_param("~driveGeometry/trackWidth", "0")
self.distance_per_count = rospy.get_param("~driveGeometry/distancePerCount", "0")
self.ignore_proximity = rospy.get_param("~ignoreProximity", False);
self.ignore_cliff_sensors = rospy.get_param("~ignoreCliffSensors", False);
self.robotParamChanged = False
# Get motor relay numbers for use later in _HandleUSBRelayStatus if USB Relay is in use:
self.relayExists = rospy.get_param("~usbRelayInstalled", False)
if self.relayExists:
# I think it is better to get these once than on every run of _HandleUSBRelayStatus
self.usbLeftMotorRelayLabel = rospy.get_param("~usbLeftMotorRelayLabel", "")
self.usbRightMotorRelayLabel = rospy.get_param("~usbRightMotorRelayLabel", "")
rospy.loginfo("Waiting for USB Relay find_relay service to start . . .")
rospy.wait_for_service('/arlobot_usbrelay/find_relay')
rospy.loginfo("USB Relay find_relay service started.")
try:
find_relay = rospy.ServiceProxy('/arlobot_usbrelay/find_relay', FindRelay)
self.leftMotorRelay = find_relay(self.usbLeftMotorRelayLabel)
self.rightMotorRelay = find_relay(self.usbRightMotorRelayLabel)
if self.leftMotorRelay.foundRelay and self.leftMotorRelay.foundRelay:
rospy.loginfo("Left = " + str(self.leftMotorRelay.relayNumber) + " & Right = " + str(
self.rightMotorRelay.relayNumber))
else:
self.relayExists = False
except rospy.ServiceException as e:
rospy.loginfo("Service call failed: %s" % e)
rospy.Subscriber("arlobot_usbrelay/usbRelayStatus", usbRelayStatus,
self._handle_usb_relay_status) # Safety Shutdown
# Subscriptions
rospy.Subscriber("cmd_vel", Twist, self._handle_velocity_command) # Is this line or the below bad redundancy?
rospy.Subscriber("arlobot_safety/safetyStatus", arloSafety, self._safety_shutdown) # Safety Shutdown
# Publishers
self._SerialPublisher = rospy.Publisher('serial', String, queue_size=10)
self._pirPublisher = rospy.Publisher('~pirState', Bool, queue_size=1) # for publishing PIR status
self._arlo_status_publisher = rospy.Publisher('arlo_status', arloStatus, queue_size=1)
# IF the Odometry Transform is done with the robot_pose_ekf do not publish it,
# but we are not using robot_pose_ekf, because it does nothing for us if you don't have a full IMU!
self._OdometryTransformBroadcaster = tf.TransformBroadcaster() # REMOVE this line if you use robot_pose_ekf
self._OdometryPublisher = rospy.Publisher("odom", Odometry, queue_size=10)
# We don't need to broadcast a transform, as it is static and contained within the URDF files
# self._SonarTransformBroadcaster = tf.TransformBroadcaster()
self._UltraSonicPublisher = rospy.Publisher("ultrasonic_scan", LaserScan, queue_size=10)
self._InfraredPublisher = rospy.Publisher("infrared_scan", LaserScan, queue_size=10)
# You can use the ~/metatron/scripts/find_propeller.sh script to find this, and
# You can set it by running this before starting this:
# rosparam set /arlobot/port $(~/metatron/scripts/find_propeller.sh)
port = rospy.get_param("~port", "/dev/ttyUSB1")
baud_rate = int(rospy.get_param("~baudRate", 115200))
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baud_rate))
self._SerialDataGateway = SerialDataGateway(port, baud_rate, self._handle_received_line)
self._OdomStationaryBroadcaster = OdomStationaryBroadcaster(self._broadcast_static_odometry_info)
def _handle_received_line(self, line): # This is Propeller specific
"""
This will run every time a line is received over the serial port (USB)
from the Propeller board and will send the data to the correct function.
"""
self._Counter += 1
self._serialTimeout = 0
# rospy.logdebug(str(self._Counter) + " " + line)
# if self._Counter % 50 == 0:
self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
if len(line) > 0:
line_parts = line.split('\t')
# We should broadcast the odometry no matter what. Even if the motors are off, or location is useful!
if line_parts[0] == 'o':
self._broadcast_odometry_info(line_parts)
return
if line_parts[0] == 'i':
self._initialize_drive_geometry(line_parts)
return
if line_parts[0] == 's': # Arlo Status info, such as sensors.
# rospy.loginfo("Propeller: " + line)
self._broadcast_arlo_status(line_parts)
return
def _broadcast_arlo_status(self, line_parts):
arlo_status = arloStatus()
# Order from ROS Interface for ArloBot.c
# dprint(term, "s\t%d\t%d\t%d\t%d\t%d\n", safeToProceed, safeToRecede, Escaping, abd_speedLimit, abdR_speedLimit);
if int(line_parts[1]) == 1:
arlo_status.safeToProceed = True
else:
arlo_status.safeToProceed = False
if int(line_parts[2]) == 1:
arlo_status.safeToRecede = True
else:
arlo_status.safeToRecede = False
if int(line_parts[3]) == 1:
arlo_status.Escaping = True
else:
arlo_status.Escaping = False
arlo_status.abd_speedLimit = int(line_parts[4])
arlo_status.abdR_speedLimit = int(line_parts[5])
arlo_status.Heading = self.lastHeading
arlo_status.gyroHeading = self.alternate_heading
arlo_status.minDistanceSensor = int(line_parts[6])
left_motor_voltage = (15 / 4.69) * float(line_parts[7])
right_motor_voltage = (15 / 4.69) * float(line_parts[8])
arlo_status.robotBatteryLevel = 12.0
if left_motor_voltage < 1:
arlo_status.leftMotorPower = False
else:
arlo_status.leftMotorPower = True
arlo_status.robotBatteryLevel = left_motor_voltage
self._leftMotorPower = arlo_status.leftMotorPower
if right_motor_voltage < 1:
arlo_status.rightMotorPower = False
else:
arlo_status.rightMotorPower = True
arlo_status.robotBatteryLevel = right_motor_voltage
self._rightMotorPower = arlo_status.rightMotorPower
# 11.6 volts is the cutoff for an SLA battery.
if arlo_status.robotBatteryLevel < 12:
arlo_status.robotBatteryLow = True
else:
arlo_status.robotBatteryLow = False
arlo_status.laptopBatteryPercent = self._laptop_battery_percent
arlo_status.acPower = self._acPower
self._arlo_status_publisher.publish(arlo_status)
def _handle_usb_relay_status(self, status):
"""
Update motor status based on real data from arlobot_usbrelay topic
"""
# This should never get called otherwise, but just in case the relay is in use but the motors are not.
if self.relayExists:
# print (status.relayOn)
# print (status.relayOn[self.leftMotorRelay.relayNumber])
# print (status.relayOn[self.leftMotorRelay.relayNumber])
# Zero indexed arrays!
if (status.relayOn[self.leftMotorRelay.relayNumber - 1] and
status.relayOn[self.rightMotorRelay.relayNumber - 1]):
self._motorsOn = True
else:
self._motorsOn = False
def _safety_shutdown(self, status):
"""
Shut down the motors if the SafeToOperate topic goes false.
Set unPlugging variable to allow for safe unplug operation.
"""
self._unPlugging = status.unPlugging
self._SafeToOperate = status.safeToGo
self._acPower = status.acPower
self._laptop_battery_percent = status.laptopBatteryPercent
if not self._SafeToOperate:
if self._motorsOn:
rospy.loginfo("Safety Shutdown initiated")
self._reset_serial_connection()
def _reset_serial_connection(self):
if self._motorsOn:
self._switch_motors(False)
# Wait for the motors to shut off
while self._motorsOn:
time.sleep(1)
# Reset the propeller board, otherwise there are problems
# if you bring up the motors again while it has been operating
self._serialAvailable = False
rospy.loginfo("Serial Data Gateway stopping . . .")
try:
self._SerialDataGateway.Stop()
except AttributeError:
rospy.loginfo("Attempt to start nonexistent Serial device.")
rospy.loginfo("Serial Data Gateway stopped.")
rospy.loginfo("5 second pause to let Activity Board settle after serial port reset . . .")
time.sleep(5) # Give it time to settle.
self.startSerialPort()
def _broadcast_odometry_info(self, line_parts):
"""
Broadcast all data from propeller monitored sensors on the appropriate topics.
"""
# If we got this far, we can assume that the Propeller board is initialized and the motors should be on.
# The _switch_motors() function will deal with the _SafeToOparete issue
if not self._motorsOn:
self._switch_motors(True)
parts_count = len(line_parts)
# rospy.logwarn(partsCount)
if parts_count != 8: # Just discard short/long lines, increment this as lines get longer
pass
x = float(line_parts[1])
y = float(line_parts[2])
# 3 is odom based heading and 4 is gyro based
theta = float(line_parts[3]) # On ArloBot odometry derived heading works best.
alternate_theta = float(line_parts[4])
vx = float(line_parts[5])
omega = float(line_parts[6])
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
ros_now = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
# This transform conflicts with transforms built into the Turtle stack
# http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
# This is done in/with the robot_pose_ekf because it can integrate IMU/gyro data
# using an "extended Kalman filter"
# REMOVE this "line" if you use robot_pose_ekf
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
ros_now,
"base_footprint",
"odom"
)
# next, we will publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = ros_now
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
# Save last X, Y and Heading for reuse if we have to reset:
self.lastX = x
self.lastY = y
self.lastHeading = theta
self.alternate_heading = alternate_theta
# robot_pose_ekf needs these covariances and we may need to adjust them.
# From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py
# However, this is not needed because we are not using robot_pose_ekf
# odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0,
# 0, 1e-3, 0, 0, 0, 0,
# 0, 0, 1e6, 0, 0, 0,
# 0, 0, 0, 1e6, 0, 0,
# 0, 0, 0, 0, 1e6, 0,
# 0, 0, 0, 0, 0, 1e3]
#
# odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0,
# 0, 1e-3, 0, 0, 0, 0,
# 0, 0, 1e6, 0, 0, 0,
# 0, 0, 0, 1e6, 0, 0,
# 0, 0, 0, 0, 1e6, 0,
# 0, 0, 0, 0, 0, 1e3]
self._OdometryPublisher.publish(odometry)
# Joint State for Turtlebot stack
# Note without this transform publisher the wheels will
# be white, stuck at 0, 0, 0 and RVIZ will tell you that
# there is no transform from the wheel_links to the base_
# However, instead of publishing a stream of pointless transforms,
# I have made the joint static in the URDF like this:
# create.urdf.xacro:
# <joint name="right_wheel_joint" type="fixed">
# NOTE This may prevent Gazebo from working with this model
# Here is the old Joint State in case you want to restore it:
# js = JointState(name=["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"],
# position=[0, 0, 0, 0], velocity=[0, 0, 0, 0], effort=[0, 0, 0, 0])
# js.header.stamp = ros_now
# self.joint_states_pub.publish(js)
# Fake laser from "PING" Ultrasonic Sensor and IR Distance Sensor input:
# http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF
# Use:
# roslaunch arlobot_rviz_launchers view_robot.launch
# to view this well for debugging and testing.
# The purpose of this is two fold:
# 1. It REALLY helps adjusting values in the Propeller and ROS
# when I can visualize the sensor output in RVIZ!
# For this purpose, a lot of the parameters are a matter of personal taste.
# Whatever makes it easiest to visualize is best.
# 2. I want to allow AMCL to use this data to avoid obstacles that the Kinect/Xtion miss.
# For the second purpose, some of the parameters here may need to be tweaked,
# to adjust how large an object looks to AMCL.
# Note that we should also adjust the distance at which AMCL takes this data
# into account either here or elsewhere.
# Transform: http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
# We do not need to broadcast a transform,
# because it is static and contained within the URDF files now.
# Here is the static transform for reference anyway:
# self._SonarTransformBroadcaster.sendTransform(
# (0.1, 0.0, 0.2),
# (0, 0, 0, 1),
# ros_now,
# "sonar_laser",
# "base_link"
# )
# Some help:
# http://goo.gl/ZU9XrJ
# TODO: I'm doing this all in degrees and then converting to Radians later.
# Is there any way to do this in Radians?
# I just don't know how to create and fill an array with "Radians"
# since they are not rational numbers, but multiples of PI, thus the degrees.
num_readings = 360 # How about 1 per degree?
#num_reeading_multiple = 2 # We have to track this so we know where to put the readings!
#num_readings = 360 * num_reeading_multiple
laser_frequency = 100 # I'm not sure how to decide what to use here.
# This is the fake distance to set all empty slots, and slots we consider "out of range"
artificial_far_distance = 10
#ranges = [1] * num_readings # Fill array with fake "1" readings for testing
# Fill array with artificial_far_distance (not 0) and then overlap with real readings
ping_ranges = [artificial_far_distance] * num_readings
# If we use 0, then it won't clear the obstacles when we rotate away,
# because costmap2d ignores 0's and Out of Range!
# Fill array with artificial_far_distance (not 0) and then overlap with real readings
ir_ranges = [artificial_far_distance] * num_readings
# New idea here:
# First, I do not think that this can be used for reliable for map generation.
# If your room has objects that the Kinect
# cannot map, then you will probably need to modify the room (cover mirrors, etc.) or try
# other laser scanner options.
# SO, since we only want to use it for cost planning, we should modify the data, because
# it is easy for it to get bogged down with a lot of "stuff" everywhere.
# From:
# http://answers.ros.org/question/11446/costmaps-obstacle-does-not-clear-properly-under-sparse-environment/
# "When clearing obstacles, costmap_2d only trusts laser scans returning a definite range.
# Indoors, that makes sense. Outdoors, most scans return max range, which does not clear
# intervening obstacles. A fake scan with slightly shorter ranges can be constructed that
# does clear them out."
# SO, we need to set all "hits" above the distance we want to pay attention to to a distance very far away,
# but just within the range_max (which we can set to anything we want),
# otherwise costmap will not clear items!
# Also, 0 does not clear anything! So if we rotate, then it gets 0 at that point, and ignores it,
# so we need to fill the unused slots with long distances.
# NOTE: This does cause a "circle" to be drawn around the robot at the "artificalFarDistance",
# but it shouldn't be a problem because we set
# artificial_far_distance to a distance greater than the planner uses.
# So while it clears things, it shouldn't cause a problem, and the Kinect should override it for things
# in between.
# Use:
# roslaunch arlobot_rviz_launchers view_robot.launch
# to view this well for debugging and testing.
# Note that sensor orientation is important here!
# If you have a different number or aim them differently this will not work!
# TODO: Tweak this value based on real measurements!
# TODO: Use both IR and PING sensors?
# The offset between the pretend sensor location in the URDF
# and real location needs to be added to these values. This may need to be tweaked.
sensor_offset = 0.217 # Measured, Calculated: 0.22545
# This will be the max used range, anything beyond this is set to "artificial_far_distance"
max_range_accepted = .5
# max_range_accepted Testing:
# TODO: More tweaking here could be done.
# I think it is a trade-off, so there is no end to the adjustment that could be done.
# I did a lot of testing with gmappingn while building a map.
# Obviously this would be slightly different from using a map we do not update.
# It seems there are so many variables here that testing is difficult.
# We could find one number works great in one situation but is hopeless in another.
# Having a comprehensive test course to test in multiple modes for every possible value would be great,
# but I think it would take months! :)
# REMEMBER, the costmap only pays attention out to a certain set
# for obstacle_range in costmap_common_params.yaml anyway.
# Here are my notes for different values of "max_range_accepted":
# 1 - looks good, and works ok, but
# I am afraid that the costmap gets confused with things popping in and out of sight all of the time,
# causing undue wandering.
# 2 - This producing less wandering due to things popping in and out of the field of view,
# BUT it also shows that we get odd affects at longer distances. i.e.
# A doorframe almost always has a hit right in the middle of it.
# In a hallway there is often a hit in the middle about 1.5 meters out.
# .5 - This works very well to have the PING data ONLY provide obstacle avoidance,
# and immediately forget about said obstacles.
# This prevents the navigation stack from fighting with the Activity Board code's
# built in safety stops, and instead navigate around obstacles before the Activity Board
# code gets involved (other than to set speed reductions).
# The only down side is if you tell ArloBot to go somewhere that he cannot due to low obstacles,
# he will try forever. He won't just bounce off of the obstacle,
# but he will keep trying it and then go away, turn around,
# and try again over and over. He may even start wandering around
# the facility trying to find another way in,
# but he will eventually come back and try it again.
# I'm not sure what the solution to this is though,
# because avoiding low lying obstacles and adding low lying
# features to the map are really two different things.
# I think if this is well tuned to avoid low lying obstacles it
# probably will not work well for mapping features.
# IF we could map features with the PING sensors, we wouldn't need the 3D sensor. :)
# TODO: One option may be more PING sensors around back.
# Right now when the robot spins, it clears the obstacles behind it,
# because there are fewer sensors on the back side.
# If the obstacle was seen all of the way around the robot, in the same spot,
# it may stop returning to the same location as soon as it turns around?
# NOTE: The bump sensors on Turtlebot mark but do not clear.
# I'm not sure how that works out. It seems like every bump would
# end up being a "blot" in the landscape never to be returned to,
# but maybe there is something I am missing?
# NOTE: Could this be different for PING vs. IR?
# Currently I'm not using IR! Just PING. The IR is not being used by costmap.
# It is here for seeing in RVIZ, and the Propeller board uses it for emergency stopping,
# but costmap isn't watching it at the moment. I think it is too erratic for that.
try:
sensor_data = json.loads(line_parts[7])
except:
return
ping = [artificial_far_distance] * 10
ir = [artificial_far_distance] * len(ping)
# Convert cm to meters and add offset
for i in range(0, len(ping)):
# ping[0] = (int(line_parts[7]) / 100.0) + sensor_offset
ping[i] = (sensor_data.get('p' + str(i), artificial_far_distance * 100) / 100.0) + sensor_offset
# Set to "out of range" for distances over "max_range_accepted" to clear long range obstacles
# and use this for near range only.
if ping[i] > max_range_accepted:
# Be sure "ultrasonic_scan.range_max" is set higher than this or
# costmap will ignore these and not clear the cost map!
ping[i] = artificial_far_distance
ir[i] = (sensor_data.get('i' + str(i), artificial_far_distance * 100) / 100.0) + sensor_offset # Convert cm to meters and add offset
# The sensors are 11cm from center to center at the front of the base plate.
# The radius of the base plate is 22.545 cm
# = 28 degree difference (http://ostermiller.org/calc/triangle.html)
sensor_seperation = 28
# Spread code: NO LONGER USED
# TODO: This could make sense to return to if used properly,
# allowing obstacles to "fill" the space and smoothly move "around"
# the robot as it rotates and objects move across the view of the PING
# sensors, instead of "jumping" from one point to the next.
# # "sensor_spread" is how wide we expand the sensor "point" in the fake laser scan.
# # For the purpose of obstacle avoidance, I think this can actually be a single point,
# # Since the costmap inflates these anyway.
#
# #One issue I am having is it seems that the "ray trace" to the maximum distance
# #may not line up with near hits, so that the global cost map is not being cleared!
# #Switching from a "spread" to a single point may fix this?
# #Since the costmap inflates obstacles anyway, we shouldn't need the spread should we?
#
# #sensor_spread = 10 # This is how wide of an arc (in degrees) to paint for each "hit"
# #sensor_spread = 2 # Testing. I think it has to be even numbers?
#
# #NOTE:
# #This assumes that things get bigger as they are further away. This is true of the PING's area,
# #and while it may or may not be true of the object the PING sees, we have no way of knowing if
# #the object fills the ping's entire field of view or only a small part of it, a "hit" is a "hit".
# #However for the IR sensor, the objects are points, that are the same size regardless of distance,
# #so we are clearly inflating them here.
#
# for x in range(180 - sensor_spread / 2, 180 + sensor_spread / 2):
# PINGranges[x] = ping[5] # Rear Sensor
# IRranges[x] = ir[5] # Rear Sensor
#
# for x in range((360 - sensor_seperation * 2) - sensor_spread / 2,
# (360 - sensor_seperation * 2) + sensor_spread / 2):
# PINGranges[x] = ping[4]
# IRranges[x] = ir[4]
#
# for x in range((360 - sensor_seperation) - sensor_spread / 2,
# (360 - sensor_seperation) + sensor_spread / 2):
# PINGranges[x] = ping[3]
# IRranges[x] = ir[3]
#
# for x in range(360 - sensor_spread / 2, 360):
# PINGranges[x] = ping[2]
# IRranges[x] = ir[2]
# # Crosses center line
# for x in range(0, sensor_spread /2):
# PINGranges[x] = ping[2]
# IRranges[x] = ir[2]
#
# for x in range(sensor_seperation - sensor_spread / 2, sensor_seperation + sensor_spread / 2):
# PINGranges[x] = ping[1]
# IRranges[x] = ir[1]
#
# for x in range((sensor_seperation * 2) - sensor_spread / 2, (sensor_seperation * 2) + sensor_spread / 2):
# PINGranges[x] = ping[0]
# IRranges[x] = ir[0]
# Single Point code:
#for x in range(180 - sensor_spread / 2, 180 + sensor_spread / 2):
ping_ranges[180 + sensor_seperation * 2] = ping[5]
ir_ranges[180 + sensor_seperation * 2] = ir[5]
ping_ranges[180 + sensor_seperation] = ping[6]
ir_ranges[180 + sensor_seperation] = ir[6]
ping_ranges[180] = ping[7] # Rear Sensor
ir_ranges[180] = ir[7] # Rear Sensor
ping_ranges[180 - sensor_seperation] = ping[8]
ir_ranges[180 - sensor_seperation] = ir[8]
ping_ranges[180 - sensor_seperation * 2] = ping[9]
ir_ranges[180 - sensor_seperation * 2] = ir[9]
# for x in range((360 - sensor_seperation * 2) - sensor_spread / 2,
# (360 - sensor_seperation * 2) + sensor_spread / 2):
ping_ranges[360 - sensor_seperation * 2] = ping[4]
ir_ranges[360 - sensor_seperation * 2] = ir[4]
# for x in range((360 - sensor_seperation) - sensor_spread / 2,
# (360 - sensor_seperation) + sensor_spread / 2):
ping_ranges[360 - sensor_seperation] = ping[3]
ir_ranges[360 - sensor_seperation] = ir[3]
#for x in range(360 - sensor_spread / 2, 360):
#PINGranges[x] = ping[2]
#IRranges[x] = ir[2]
# Crosses center line
#for x in range(0, sensor_spread /2):
ping_ranges[0] = ping[2]
ir_ranges[0] = ir[2]
#for x in range(sensor_seperation - sensor_spread / 2, sensor_seperation + sensor_spread / 2):
ping_ranges[sensor_seperation] = ping[1]
ir_ranges[sensor_seperation] = ir[1]
#for x in range((sensor_seperation * 2) - sensor_spread / 2, (sensor_seperation * 2) + sensor_spread / 2):
ping_ranges[sensor_seperation * 2] = ping[0]
ir_ranges[sensor_seperation * 2] = ir[0]
# LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html
ultrasonic_scan = LaserScan()
infrared_scan = LaserScan()
ultrasonic_scan.header.stamp = ros_now
infrared_scan.header.stamp = ros_now
ultrasonic_scan.header.frame_id = "ping_sensor_array"
infrared_scan.header.frame_id = "ir_sensor_array"
# For example:
#scan.angle_min = -45 * M_PI / 180; // -45 degree
#scan.angle_max = 45 * M_PI / 180; // 45 degree
# if you want to receive a full 360 degrees scan,
# you should try setting min_angle to -pi/2 and max_angle to 3/2 * pi.
# Radians: http://en.wikipedia.org/wiki/Radian#Advantages_of_measuring_in_radians
ultrasonic_scan.angle_min = 0
infrared_scan.angle_min = 0
#ultrasonic_scan.angle_max = 2 * 3.14159 # Full circle # Letting it use default, which I think is the same.
#infrared_scan.angle_max = 2 * 3.14159 # Full circle # Letting it use default, which I think is the same.
#ultrasonic_scan.scan_time = 3 # I think this is only really applied for 3D scanning
#infrared_scan.scan_time = 3 # I think this is only really applied for 3D scanning
# Make sure the part you divide by num_readings is the same as your angle_max!
# Might even make sense to use a variable here?
ultrasonic_scan.angle_increment = (2 * 3.14) / num_readings
infrared_scan.angle_increment = (2 * 3.14) / num_readings
ultrasonic_scan.time_increment = (1 / laser_frequency) / num_readings
infrared_scan.time_increment = (1 / laser_frequency) / num_readings
# From: http://www.parallax.com/product/28015
# Range: approximately 1 inch to 10 feet (2 cm to 3 m)
# This should be adjusted based on the imaginary distance between the actual laser
# and the laser location in the URDF file.
# in Meters Distances below this number will be ignored REMEMBER the offset!
ultrasonic_scan.range_min = 0.02
# in Meters Distances below this number will be ignored REMEMBER the offset!
infrared_scan.range_min = 0.02
# This has to be above our "artificial_far_distance",
# otherwise "hits" at artificial_far_distance will be ignored,
# which means they will not be used to clear the cost map!
# in Meters Distances above this will be ignored
ultrasonic_scan.range_max = artificial_far_distance + 1
# in Meters Distances above this will be ignored
infrared_scan.range_max = artificial_far_distance + 1
ultrasonic_scan.ranges = ping_ranges
infrared_scan.ranges = ir_ranges
# "intensity" is a value specific to each laser scanner model.
# It can safely be ignored
self._UltraSonicPublisher.publish(ultrasonic_scan)
self._InfraredPublisher.publish(infrared_scan)
def _write_serial(self, message):
self._SerialPublisher.publish(String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
def start(self):
self._OdomStationaryBroadcaster.Start()
self.startSerialPort()
self._serialTimeout = 0
def startSerialPort(self):
rospy.loginfo("Serial Data Gateway starting . . .")
try:
self._SerialDataGateway.Start()
except:
rospy.loginfo("ARLO SERIAL PORT Start Error")
reset_usb_script = os.path.expanduser("~/metatron/scripts/callRestUSB.sh")
if os.path.isfile(reset_usb_script):
rospy.loginfo("RESETING USB PORTS")
rospy.loginfo(reset_usb_script)
devnull = open(os.devnull, 'w')
subprocess.call(["/bin/bash", reset_usb_script], stdout=devnull, stderr=devnull)
# The reset should have shut off the motors,
# and allowing the stop method to try turning off the motors after the
# USB port reset will make the arlobot_usbrelay node hang
self._motorsOn = False
# At this point we have to restart the node.
# The respawn atribute in the launch file should handle this.
raise SystemExit(0)
rospy.loginfo("Serial Data Gateway started.")
self._serialAvailable = True
def stop(self):
"""
Called by ROS on shutdown.
Shut off motors, record position and reset serial port.
"""
rospy.loginfo("Stopping")
self._SafeToOperate = False # Prevent threads fighting
if self._motorsOn:
# arlobot_usbrelay will also shut off all relays after a delay, but better safe than sorry!
self._switch_motors( False)
# Save last position in parameter server in case we come up again without restarting roscore!
rospy.set_param('lastX', self.lastX)
rospy.set_param('lastY', self.lastY)
rospy.set_param('lastHeading', self.lastHeading)
time.sleep(3) # Give the motors time to shut off
self._serialAvailable = False
rospy.loginfo("_SerialDataGateway stopping . . .")
try:
self._SerialDataGateway.Stop()
except AttributeError:
rospy.loginfo("Attempt to start nonexistent Serial device.")
rospy.loginfo("_SerialDataGateway stopped.")
self._OdomStationaryBroadcaster.Stop()
def _handle_velocity_command(self, twist_command): # This is Propeller specific
""" Handle movement requests. """
# NOTE: turtlebot_node has a lot of code under its cmd_vel function
# to deal with maximum and minimum speeds,
# which are dealt with in ArloBot on the Activity Board itself in the Propeller code.
if self._clear_to_go("forGeneralUse"):
v = twist_command.linear.x # m/s
omega = twist_command.angular.z # rad/s
# rospy.logdebug("Handling twist command: " + str(v) + "," + str(omega))
message = 's,%.3f,%.3f\r' % (v, omega)
self._write_serial(message)
if self.robotParamChanged:
if (self.ignore_proximity):
ignore_proximity = 1
else:
ignore_proximity = 0
if (self.ignore_cliff_sensors):
ignore_cliff_sensors = 1
else:
ignore_cliff_sensors = 0
message = 'd,%f,%f,%d,%d\r' % (self.track_width, self.distance_per_count, ignore_proximity, ignore_cliff_sensors)
self.robotParamChanged = False
self._write_serial(message)
elif self._clear_to_go("to_stop"):
message = 's,0.0,0.0\r' # Tell it to be still if it is not safe to operate
# rospy.logdebug("Sending speed command message: " + message)
self._write_serial(message)
def _initialize_drive_geometry(self, line_parts):
""" Send parameters from YAML file to Propeller board. """
if self._SafeToOperate:
if (self.ignore_proximity):
ignore_proximity = 1
else:
ignore_proximity = 0
if (self.ignore_cliff_sensors):
ignore_cliff_sensors = 1
else:
ignore_cliff_sensors = 0
message = 'd,%f,%f,%d,%d,%f,%f,%f\r' % (self.track_width, self.distance_per_count, ignore_proximity, ignore_cliff_sensors, self.lastX, self.lastY, self.lastHeading)
rospy.logdebug("Sending drive geometry params message: " + message)
self._write_serial(message)
else:
if int(line_parts[1]) == 1:
self._pirPublisher.publish(True)
else:
self._pirPublisher.publish(False)
def _broadcast_static_odometry_info(self):
"""
Broadcast last known odometry and transform while propeller board is offline
so that ROS can continue to track status
Otherwise things like gmapping will fail when we loose our transform and publishing topics
"""
if not self._motorsOn: # Use motor status to decide when to broadcast static odometry:
x = self.lastX
y = self.lastY
theta = self.lastHeading
vx = 0 # If the motors are off we will assume the robot is still.
omega = 0 # If the motors are off we will assume the robot is still.
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
ros_now = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
# This transform conflicts with transforms built into the Turtle stack
# http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
# This is done in/with the robot_pose_ekf because it can integrate IMU/gyro data
# using an "extended Kalman filter"
# REMOVE this "line" if you use robot_pose_ekf
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
ros_now,
"base_footprint",
"odom"
)
# next, we will publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = ros_now
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
def _switch_motors(self, state):
""" Switch Motors on and off as needed. """
# Relay control was moved to its own package
if self.relayExists:
if not self._SwitchingMotors: # Prevent overlapping runs
self._SwitchingMotors = True
# Switch "on" to "off" if not safe to operate,
# then we can just pass state to arlobot_usbrelay
if not self._SafeToOperate:
state = False
rospy.wait_for_service('/arlobot_usbrelay/toggle_relay')
rospy.loginfo("Switching motors.")
try:
toggle_relay = rospy.ServiceProxy('/arlobot_usbrelay/toggle_relay', ToggleRelay)
left_relay_result = toggle_relay(self.usbLeftMotorRelayLabel, state)
right_relay_result = toggle_relay(self.usbRightMotorRelayLabel, state)
if left_relay_result.toggleSuccess and right_relay_result.toggleSuccess:
self._motorsOn = True
else:
self._motorsOn = False
except rospy.ServiceException as e:
rospy.loginfo("Service call failed: %s" % e)
self._SwitchingMotors = False
else: # If no automated motor control exists, just set the state blindly.
self._motorsOn = state
def watchDog(self):
while not rospy.is_shutdown():
if self._serialAvailable:
self._serialTimeout += 1
else:
self._serialTimeout = 0
#rospy.loginfo("Serial Timeout = " + str(self._serialTimeout))
if self._serialTimeout > 19:
rospy.loginfo("Watchdog Timeout Reset initiated")
self._reset_serial_connection()
if self._unPlugging or self._wasUnplugging:
self.UnplugRobot()
old_track_width = self.track_width
self.track_width = rospy.get_param("~driveGeometry/trackWidth", "0")
if not old_track_width == self.track_width:
self.robotParamChanged = True
old_distance_per_count = self.distance_per_count
self.distance_per_count = rospy.get_param("~driveGeometry/distancePerCount", "0")
if not old_distance_per_count == self.distance_per_count:
self.robotParamChanged = True
old_ignore_proximity = self.ignore_proximity
self.ignore_proximity = rospy.get_param("~ignoreProximity", False);
if not old_ignore_proximity == self.ignore_proximity:
self.robotParamChanged = True
self.r.sleep()
old_ignore_cliff_sensors = self.ignore_cliff_sensors
self.ignore_cliff_sensors = rospy.get_param("~ignoreCliffSensors", False);
if not old_ignore_cliff_sensors == self.ignore_cliff_sensors:
self.robotParamChanged = True
def UnplugRobot(self):
if self._unPlugging and \
not self._wasUnplugging and \
self._clear_to_go("forUnplugging"):
# We will only do this once, and let it continue until AC power is disconnected
self._wasUnplugging = True
# Slow backup until unplugged
# This should be a slow backward crawl
# -0.01 is about as slow as possible
# -0.02 works more reliably
rospy.loginfo("Unplugging!")
message = 's,-0.02,0.0\r'
self._write_serial(message)
# Once we are unplugged, stop the robot before returning control to handle_velocity_command
# And we only need permission to stop at this point.
if self._wasUnplugging and \
not self._acPower and \
self._serialAvailable:
rospy.loginfo("Unplugging complete")
message = 's,0.0,0.0\r'
self._wasUnplugging = False
self._write_serial(message)
# Finally, if we were unplugging, but something went wrong, we should stop the robot
# Since no one else will do this while we have the "_wasUnplugging" variable
# set.
if self._wasUnplugging and \
not self._clear_to_go("forUnplugging") and \
self._serialAvailable:
message = 's,0.0,0.0\r'
self._wasUnplugging = False
self._write_serial(message)
# Considlate "clear to go" requirements here.
def _clear_to_go(self, forWhat):
return_value = False
# Required for all operations
if self._serialAvailable and \
self._SafeToOperate and \
self._motorsOn and \
self._leftMotorPower and \
self._rightMotorPower:
return_value = True
# Negations by use case
if forWhat == "forUnplugging":
# Unpugging should only happen if AC is connected
if not self._acPower:
return_value = False
if forWhat == "forGeneralUse":
# The handle_velocity_command should only operate if the robot is unplugged,
# and the unplugging function of the Watchdog process is not in control
if self._acPower or \
self._wasUnplugging:
return_value = False
# Special Cases
if forWhat == "to_stop":
# handle_velocity_command can send a STOP if it is unsafe to do anythging else,
# but the serial connection is up,
# as long as the unplugging operation is not in progress
# This should bring the robot to a halt in response to any other requset,
# if anything is amiss.
if self._serialAvailable and \
not self._wasUnplugging:
return_value = True
else:
return_value = False
return return_value
class Arduino(object):
'''
Helper class for communicating with an Arduino board over serial port
'''
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
#rospy.logdebug(str(self._Counter) + " " + line)
#if (self._Counter % 50 == 0):
self._Publisher.publish(String(str(self._Counter) + " " + line))
if (len(line) > 0):
lineParts = line.split('\t')
if (lineParts[0] == 'o'):
self._BroadcastOdometryInfo(lineParts)
return
if (lineParts[0] == "InitializeSpeedController"):
# controller requesting initialization
self._InitializeDriveMotorGains()
return
if (lineParts[0] == "InitializeBatteryMonitor"):
# controller requesting initialization
self._InitializeBatteryMonitor()
return
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
theta = float(lineParts[3])
vx = float(lineParts[4])
omega = float(lineParts[5])
#quaternion = tf.transformations.quaternion_about_axis(theta, (0,0,1))
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# first, we'll publish the transform over tf
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_link", "odom")
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
#rospy.loginfo(odometry)
except:
rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
self._Counter = 0
rospy.init_node('arduino')
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
self._Publisher = rospy.Publisher('serial', String)
self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
self._OdometryPublisher = rospy.Publisher("odom", Odometry)
self._SetDriveGainsService = rospy.Service('setDriveControlGains',
SetDriveControlGains,
self._HandleSetDriveGains)
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
def _HandleVelocityCommand(self, twistCommand):
""" Handle movement requests. """
v = twistCommand.linear.x # m/s
omega = twistCommand.angular.z # rad/s
rospy.loginfo("Handling twist command: " + str(v) + "," + str(omega))
message = 's %d %d %d %d \r' % self._GetBaseAndExponents((v, omega))
rospy.logdebug("Sending speed command message: " + message)
self._SerialDataGateway.Write(message)
def _InitializeDriveMotorGains(self):
kp = rospy.get_param("~driveMotorGains/kp", "0")
ki = rospy.get_param("~driveMotorGains/ki", "0")
kd = rospy.get_param("~driveMotorGains/kd", "0")
driveGains = (kp, ki, kd)
self._WriteDriveGains(driveGains)
def _HandleSetDriveGains(self, request):
""" Handle the setting of the drive gains (PID). """
# We persist the new values in the parameter server
rospy.set_param("~driveMotorGains", {
'kp': request.kp,
'ki': request.ki,
'kd': request.kd
})
driveGains = (request.kp, request.ki, request.kd)
self._WriteDriveGains(driveGains)
return SetDriveControlGainsResponse()
def _WriteDriveGains(self, driveGains):
""" Writes the drive gains (PID) to the Arduino controller. """
rospy.logdebug("Handling 'SetDriveGains'; received parameters " +
str(driveGains))
message = 'SpeedControllerGains %d %d %d %d %d %d\r' % self._GetBaseAndExponents(
driveGains)
rospy.logdebug("Sending speed controller gains message: " + message)
self._SerialDataGateway.Write(message)
def _InitializeBatteryMonitor(self):
voltageTooLowlimit = rospy.get_param(
"~batteryMonitorParams/voltageTooLowlimit", "12.0")
rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " +
str(voltageTooLowlimit))
message = 'BatteryMonitorParams %d %d\r' % self._GetBaseAndExponent(
voltageTooLowlimit)
rospy.logdebug("Sending battery monitor params message: " + message)
self._SerialDataGateway.Write(message)
def _GetBaseAndExponent(self, floatValue, resolution=4):
'''
Converts a float into a tuple holding two integers:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
floatValue = float(floatValue)
if (floatValue == 0.0):
return (0, 0)
else:
exponent = int(1.0 + math.log10(abs(floatValue)))
multiplier = math.pow(10, resolution - exponent)
base = int(floatValue * multiplier)
return (base, exponent - resolution)
def _GetBaseAndExponents(self, floatValues, resolution=4):
'''
Converts a list or tuple of floats into a tuple holding two integers for each float:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
baseAndExponents = []
for floatValue in floatValues:
baseAndExponent = self._GetBaseAndExponent(floatValue)
baseAndExponents.append(baseAndExponent[0])
baseAndExponents.append(baseAndExponent[1])
return tuple(baseAndExponents)
class Arduino(object):
'''
Helper class for communicating with an Arduino board over serial port
'''
CONTROLLER_RESET_REQUIRED = 0
CONTROLLER_INITIALIZING = 1
CONTROLLER_IS_READY = 2
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
#rospy.logdebug(str(self._Counter) + " " + line)
#if (self._Counter % 50 == 0):
self._SerialPublisher.publish(
String(str(self._Counter) + ", in: " + line))
if (len(line) > 0):
lineParts = line.split('\t')
self._State = Arduino.CONTROLLER_INITIALIZING
# if (self._State == Arduino.CONTROLLER_RESET_REQUIRED):
# if (lineParts[0] == "reset_done"):
# return
# else:
# self._WriteSerial('reset\r')
# return
if (self._State == Arduino.CONTROLLER_INITIALIZING):
if (lineParts[0] == "initialized"):
self._State = Arduino.CONTROLLER_IS_READY
return
elif (lineParts[0] == "InitializeDriveGeometry"):
# controller requesting initialization
self._InitializeDriveGeometry()
return
elif (lineParts[0] == "InitializeSpeedController"):
# controller requesting initialization
self._InitializeSpeedController()
return
elif (lineParts[0] == "InitializeBatteryMonitor"):
# controller requesting initialization
self._InitializeBatteryMonitor()
return
if (lineParts[0] == 'o'):
self._BroadcastOdometryInfo(lineParts)
# rospy.logwarn("Called odom")
return
if (lineParts[0] == 'b'):
# self._BroadcastBatteryInfo(lineParts)
return
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(lineParts)
# if (partsCount <= 6):
# pass
try:
# rospy.logwarn("X")
# rospy.logwarn(lineParts[1])
x = float(lineParts[1])
# rospy.logwarn(line_parts_float)
# line_parts_int = round(line_parts_float)
# rospy.logwarn(line_parts_int)
# x = line_parts_int / 1000.0
# rospy.logwarn(x)
# rospy.logwarn("Y")
# rospy.logwarn(lineParts[2])
y = float(lineParts[2])
# line_parts_int = round(line_parts_float)
# y = line_parts_int / 1000.0
# rospy.logwarn(y)
theta = float(lineParts[3])
# line_parts_int = round(line_parts_float)
# theta = line_parts_int / 1000.0
vx = float(lineParts[4])
# line_parts_int = round(line_parts_float)
# vx = line_parts_int / 1000.0
omega = float(lineParts[5])
# rospy.logwarn(line_parts_float)
# line_parts_int = round(line_parts_float)
# omega = line_parts_int / 1000.0
#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0
quaternion.y = 0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_footprint", "odom")
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
except:
# rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
rospy.logwarn("Error from odometry pub")
def _BroadcastBatteryInfo(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(lineParts)
if (partsCount < 1):
pass
try:
line_parts_float = float(lineParts[1])
rospy.logwarn("Bat 1")
lineparts_int = round(line_parts_float)
rospy.logwarn("Bat 2")
rospy.logwarn(lineparts_int)
batteryVoltage = 12
rospy.logwarn("Bat 3")
batteryState = BatteryState()
batteryState.voltage = batteryVoltage
if (batteryVoltage <= self._VoltageLowlimit):
batteryState.isLow = 1
if (batteryVoltage <= self._VoltageLowLowlimit):
batteryState.isLowLow = 1
self._BatteryStatePublisher.publish(batteryState)
rospy.loginfo(batteryState)
except:
# rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
rospy.logwarn("Error from battery")
def _WriteSerial(self, message):
self._SerialPublisher.publish(
String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
self._Counter = 0
rospy.init_node('arduino')
# rospy.logwarn("Init");
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand
) # Is this line or the below bad redundancy?
rospy.Subscriber("cmd_vel_mux/input/teleop", Twist,
self._HandleVelocityCommand
) # IS this line or the above bad redundancy?
self._SerialPublisher = rospy.Publisher('serial',
String,
queue_size=10)
self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
self._OdometryPublisher = rospy.Publisher("odom",
Odometry,
queue_size=10)
# self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
# self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")
# self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
#self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)
self._State = Arduino.CONTROLLER_RESET_REQUIRED
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
self._VoltageHighlimit = 12
self._VoltageLowlimit = 11
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
def _HandleVelocityCommand(self, twistCommand):
""" Handle movement requests. """
v = twistCommand.linear.x # m/s
omega = twistCommand.angular.z # rad/s
rospy.logwarn("Handling twist command: " + str(v) + "," + str(omega))
v = v * 1000
omega = omega * 1000
# message = 's %.3f %.3f\r' % (v, omega)
message = 's %d %d\r' % (v, omega)
rospy.logwarn(str(v) + str(omega))
rospy.logwarn("Sending speed command message: " + message)
self._WriteSerial(message)
def _InitializeDriveGeometry(self):
wheelDiameter = rospy.get_param("~driveGeometry/wheelDiameter", "0.9")
trackWidth = rospy.get_param("~driveGeometry/trackWidth", "0.1")
countsPerRevolution = rospy.get_param(
"~driveGeometry/countsPerRevolution", "2000")
wheelDiameter = wheelDiameter * 1000
trackWidth = trackWidth * 1000
#countsPerRevolution=countsperRevolution*1000;
#wheelDiameterParts = self._GetBaseAndExponent(wheelDiameter)
#trackWidthParts = self._GetBaseAndExponent(trackWidth)
message = 'dg %d %d %d\r' % (int(wheelDiameter), int(trackWidth),
int(countsPerRevolution))
rospy.logdebug("Sending drive geometry params message: " + message)
self._WriteSerial(message)
def _InitializeSpeedController(self):
velocityPParam = rospy.get_param("~speedController/velocityPParam",
"1")
velocityIParam = rospy.get_param("~speedController/velocityIParam",
"1")
turnPParam = rospy.get_param("~speedController/turnPParam", "1")
turnIParam = rospy.get_param("~speedController/turnIParam", "1")
commandTimeout = self._GetCommandTimeoutForSpeedController()
velocityPParam = velocityPParam * 1000
velocityIParam = velocityIParam * 1000
turnPParam = turnPParam * 1000
turnIParam = turnIParam * 1000
message = 'sc %d %d %d %d %d\r' % (
int(velocityPParam), int(velocityIParam), int(turnPParam),
int(turnIParam), int(commandTimeout))
#message = 'sc %f %f %f\r' % (velocityPParam, velocityIParam, turnPParam)
rospy.logdebug("Sending differential drive gains message: " + message)
self._WriteSerial(message)
#speedControllerParams = (velocityPParam, velocityIParam, turnPParam, turnIParam, commandTimeout)
#rospy.loginfo(str(speedControllerParams))
#self._WriteSpeedControllerParams(speedControllerParams)
def _GetCommandTimeoutForSpeedController(self):
"""
Returns the command timeout for the speed controller in seconds.
If no velocity command arrives for more than the specified timeout then the speed controller will stop the robot.
"""
return rospy.get_param("~speedController/commandTimeout", "0.5")
def _HandleSetDriveGains(self, request):
""" Handle the setting of the drive gains (PID). """
# We persist the new values in the parameter server
rospy.set_param(
"~speedController", {
'velocityPParam': request.velocityPParam,
'velocityPParam': request.velocityIParam,
'turnPParam': request.turnPParam,
'turnIParam': request.turnIParam
})
commandTimeout = self._GetCommandTimeoutForSpeedController()
speedControllerParams = (request.velocityPParam,
request.velocityIParam, request.turnPParam,
request.turnIParam, commandTimeout)
self._WriteSpeedControllerParams(speedControllerParams)
return SetDriveControlGainsResponse()
def _WriteSpeedControllerParams(self, speedControllerParams):
""" Writes the speed controller parameters (drive gains (PID), and command timeout) to the Arduino controller. """
rospy.logdebug(
"Handling '_WriteSpeedControllerParams'; received parameters " +
str(speedControllerParams))
message = 'SpeedCo %d %d %d %d %d %d %d %d %d %d\r' % self._GetBaseAndExponents(
speedControllerParams)
message = 'SpeedCo 763 -4 3700 -4 9750\r'
# message =
rospy.logdebug("Sending differential drive gains message: " + message)
# self._WriteSerial(message)
def _InitializeBatteryMonitor(self):
# rospy.logdebug("Initializing battery monitor. self._VoltageLowLowlimitvoltageTooLowLimit = " + str(self._VoltageLowLowlimit))
rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " +
str(self._VoltageLowlimit))
temp_high_v = self._VoltageHighlimit * 1000
self._VoltageHighlimit = temp_high_v
temp_low_v = self._VoltageLowlimit * 1000
self._VoltageLowlimit = temp_low_v
message = 'bm %d %d\r' % (self._VoltageHighlimit,
self._VoltageLowlimit)
rospy.logdebug("Sending battery monitor params message: " + message)
self._WriteSerial(message)
self._VoltageHighlimit = 12
self._VoltageLowlimit = 11
def _GetBaseAndExponent(self, floatValue, resolution=4):
'''
Converts a float into a tuple holding two integers:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
if (floatValue == 0.0):
return (0, 0)
else:
exponent = int(1.0 + math.log10(abs(floatValue)))
multiplier = math.pow(10, resolution - exponent)
base = int(floatValue * multiplier)
return (base, exponent - resolution)
def _GetBaseAndExponents(self, floatValues, resolution=4):
'''
Converts a list or tuple of floats into a tuple holding two integers for each float:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
baseAndExponents = []
for floatValue in floatValues:
baseAndExponent = self._GetBaseAndExponent(floatValue)
baseAndExponents.append(baseAndExponent[0])
baseAndExponents.append(baseAndExponent[1])
return tuple(baseAndExponents)
def __init__(self):
print "Initializing Arduino Class"
#######################################################################################################################
#Sensor variables
self.req_right_ = 0
self.req_left_ = 0
self.actual_right = 0 # vector x
self.actual_left = 0 # vector y
self.time_ = 0.0 # time difference
self.gyro_X = 0.0
self.gyro_Y = 0.0
self.gyro_Z = 0.0
self.accel_X= 0.0
self.accel_Y= 0.0
self.accel_Z= 0.0
self._Counter=0
self.imu = Imu()
self.imu.header.frame_id = "imu"
self.imu.orientation_covariance[0] = 1e6;
self.imu.orientation_covariance[1] = 0;
self.imu.orientation_covariance[2] = 0;
self.imu.orientation_covariance[3] = 0;
self.imu.orientation_covariance[4] = 1e6;
self.imu.orientation_covariance[5] = 0;
self.imu.orientation_covariance[6] = 0;
self.imu.orientation_covariance[7] = 0;
self.imu.orientation_covariance[8] = 1e-6;
self.imu.angular_velocity_covariance[0] = 1e6;
self.imu.angular_velocity_covariance[1] = 0;
self.imu.angular_velocity_covariance[2] = 0;
self.imu.angular_velocity_covariance[3] = 0;
self.imu.angular_velocity_covariance[4] = 1e6;
self.imu.angular_velocity_covariance[5] = 0;
self.imu.angular_velocity_covariance[6] = 0;
self.imu.angular_velocity_covariance[7] = 0;
self.imu.angular_velocity_covariance[8] = 1e-6;
self.imu.linear_acceleration_covariance[0] = -1;
self.imu.linear_acceleration_covariance[1] = 0;
self.imu.linear_acceleration_covariance[2] = 0;
self.imu.linear_acceleration_covariance[3] = 0;
self.imu.linear_acceleration_covariance[4] = 0;
self.imu.linear_acceleration_covariance[5] = 0;
self.imu.linear_acceleration_covariance[6] = 0;
self.imu.linear_acceleration_covariance[7] = 0;
self.imu.linear_acceleration_covariance[8] = 0;
self.vector3 = Vector3Stamped()
#######################################################################################################################
#Get serial port and baud rate of Tiva C Arduino
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))
#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate, self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
self._Req_subscriber = rospy.Subscriber('rpm_req_msg',rpm,self._Handle_rpm_request)
self._Act_publisher = rospy.Publisher('rpm_act_msg',Vector3Stamped,queue_size = 10)
self._Imu_publisher = rospy.Publisher('imu/data_raw',Imu,queue_size=10)
self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)
class R_shoulder(object):
'''
Helper class for communicating with an R_shoulder board over serial port
'''
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
#rospy.logwarn(str(self._Counter) + " " + line)
#if (self._Counter % 50 == 0):
self._SerialPublisher.publish(
String(str(self._Counter) + ", in: " + line))
if (len(line) > 0):
lineParts = line.split('\t')
if (lineParts[0] == 'p1'):
self._BroadcastJointStateinfo_P1(lineParts)
return
if (lineParts[0] == 'p2'):
self._BroadcastJointStateinfo_P2(lineParts)
return
if (lineParts[0] == 'p3'):
self._BroadcastJointStateinfo_P3(lineParts)
return
if (lineParts[0] == 'p4'):
self._BroadcastJointStateinfo_P4(lineParts)
return
if (lineParts[0] == 'p5'):
self._BroadcastJointStateinfo_P5(lineParts)
return
def _BroadcastJointStateinfo_P1(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
P1 = ((321 - float(lineParts[1])) * 0.008) - 0.25
P2 = self.left_tilt
P3 = float(lineParts[3]) # current
P4 = 0 #float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P1_MotorPublisher.publish(Motor_State)
self._left_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now(
) #.stamprospy.Time.from_sec(state.timestamp)
self._P1_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_tilt_joint" +
str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P2(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 4):
pass
try:
P1 = ((321 - float(lineParts[1])) * 0.008) - 0.12
P2 = self.right_tilt
P3 = 0 #float(lineParts[3])# current
P4 = 0 #float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P2_MotorPublisher.publish(Motor_State)
self._right_tilt_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "right_arm_tilt_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now(
) #.stamprospy.Time.from_sec(state.timestamp)
self._P2_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:right_arm_tilt_joint" +
str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P3(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
#offset = Float(-1.57)
P1 = (float(lineParts[1]) / 1000) - 0.6
P2 = self.right_lift #0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = 0 #float(lineParts[3])
P4 = 0
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P3_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
self._left_lift_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_lift_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P3_JointPublisher.publish(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_lift_joint" +
str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P4(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 7):
pass
try:
#off = 1570
P1 = float(lineParts[1]) / 1000
P2 = self.right_lift #0-((float(lineParts[2])* 0.00174532925)-1.57)
P3 = 0 #float(lineParts[3])
P4 = 0
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P4_MotorPublisher.publish(Motor_State)
#rospy.logwarn(Motor_State)
self._right_lift_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "right_arm_lift_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now()
self._P4_JointPublisher.publish(Joint_State)
except:
rospy.logwarn("Unexpected error:right_arm_lift_joint" +
str(sys.exc_info()[0]))
def _BroadcastJointStateinfo_P5(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
P1 = 0 - radians(float(lineParts[1]) / 10)
P2 = self.left_elbow
P3 = 0 #float(lineParts[3])# current
P4 = 0 #float(lineParts[4])# speed
val = [P1, P2, P3, P4]
Motor_State = MotorState()
Motor_State.id = 11
Motor_State.goal = P2
Motor_State.position = P1
Motor_State.speed = P4
Motor_State.load = P3
Motor_State.moving = 0
Motor_State.timestamp = time.time()
self.P5_MotorPublisher.publish(Motor_State)
self._left_elbow_Publisher.publish(P1)
Joint_State = JointState()
Joint_State.name = "left_arm_elbow_joint"
Joint_State.goal_pos = P2
Joint_State.current_pos = P1
Joint_State.velocity = P4
Joint_State.load = P3
Joint_State.error = P1 - P2
Joint_State.is_moving = 0
Joint_State.header.stamp = rospy.Time.now(
) #.stamprospy.Time.from_sec(state.timestamp)
self._P5_JointPublisher.publish(Joint_State)
#rospy.logwarn(Joint_State)
except:
rospy.logwarn("Unexpected error:left_arm_elbow_joint" +
str(sys.exc_info()[0]))
def _WriteSerial(self, message):
self._SerialPublisher.publish(
String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
def __init__(self, ):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
#port = rospy.get_param("~port", "/dev/ttyACM0")
#baud = int(rospy.get_param("~baud", "115200"))
#self.name = name
self.rate = rospy.get_param("~rate", 100.0)
self.fake = rospy.get_param("~sim", False)
self.cal_pan = rospy.get_param("~cal_pan", 0)
self.cal_tilt = rospy.get_param("~cal_tilt", 0)
self.cal_lift = rospy.get_param("~cal_lift", 0)
self.cal_rotate = rospy.get_param("~cal_rotate", 0)
self.cal_elbow = rospy.get_param("~cal_elbow", 0)
self.right_tilt = 0
self.left_tilt = 0
self.right_lift = 1.57
self.left_lift = 1.57
self.left_elbow = 0
#name = rospy.get_param("~name")
self._Counter = 0
rospy.init_node('r_shoulder')
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.logwarn("Starting Right shoulder with serial port: " + port +
", baud rate: " + str(baudRate))
# subscriptions
rospy.Subscriber('left_arm_tilt_joint/command', Float64,
self._HandleJoint_1_Command)
rospy.Subscriber('right_arm_tilt_joint/command', Float64,
self._HandleJoint_2_Command)
rospy.Subscriber('left_arm_lift_joint/command', Float64,
self._HandleJoint_3_Command)
rospy.Subscriber('right_arm_lift_joint/command', Float64,
self._HandleJoint_4_Command)
rospy.Subscriber('left_arm_elbow_joint/command', Float64,
self._HandleJoint_8_Command)
self._SerialPublisher = rospy.Publisher('arm_upper',
String,
queue_size=5)
self.P1_MotorPublisher = rospy.Publisher("/left_arm_tilt/motor_state",
MotorState,
queue_size=5)
self.P2_MotorPublisher = rospy.Publisher("/right_arm_tilt/motor_state",
MotorState,
queue_size=5)
self.P3_MotorPublisher = rospy.Publisher("/left_arm_lift/motor_state",
MotorState,
queue_size=5)
self.P4_MotorPublisher = rospy.Publisher("/right_arm_lift/motor_state",
MotorState,
queue_size=5)
self.P5_MotorPublisher = rospy.Publisher("/left_arm_elbow/motor_state",
MotorState,
queue_size=5)
self._P1_JointPublisher = rospy.Publisher("/left_arm_tilt_joint/state",
JointState,
queue_size=5)
self._P2_JointPublisher = rospy.Publisher(
"/right_arm_tilt_joint/state", JointState, queue_size=5)
self._P3_JointPublisher = rospy.Publisher("/left_arm_lift_joint/state",
JointState,
queue_size=5)
self._P4_JointPublisher = rospy.Publisher(
"/right_arm_lift_joint/state", JointState, queue_size=5)
self._P5_JointPublisher = rospy.Publisher(
"/left_arm_elbow_joint/state", JointState, queue_size=5)
self._right_lift_Publisher = rospy.Publisher("right_lift",
Float32,
queue_size=5)
self._right_tilt_Publisher = rospy.Publisher("right_tilt",
Float32,
queue_size=5)
self._left_lift_Publisher = rospy.Publisher("left_lift",
Float32,
queue_size=5)
self._left_tilt_Publisher = rospy.Publisher("left_tilt",
Float32,
queue_size=5)
self._left_elbow_Publisher = rospy.Publisher("left_elbow",
Float32,
queue_size=5)
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
def Start(self):
rospy.loginfo("Starting start function")
self._SerialDataGateway.Start()
message = 'r \r'
self._WriteSerial(message)
def Stop(self):
rospy.loginfo("Stopping")
message = 'r \r'
self._WriteSerial(message)
sleep(5)
self._SerialDataGateway.Stop()
def _HandleJoint_1_Command(self, Command):
""" Handle movement requests.
left_arm_tilt_joint
send message in degrees * 10
"""
v = Command.data # angel request in radians
self.left_tilt = v
v1 = int((90 - degrees(v)) / 1.6) + 27
if v1 < 83: v1 = 83 #ticks
if v1 > 160: v1 = 160 #ticks
rospy.logwarn("Handling left tilt command: " + str(v))
message = 'j6 %d \r' % (v1) #% self._GetBaseAndExponents((v1)
rospy.logwarn("Sending left_arm_tilt_joint command: " + (message))
self._WriteSerial(message)
def _HandleJoint_2_Command(self, Command): #tilt
""" Handle movement requests.
right_arm_tilt_joint
send message in degrees * 10
"""
v = Command.data # angel request in radians
self.right_tilt = v
v1 = int((90 - degrees(v)) / 1.6) + 27
if v1 < 83: v1 = 83 #ticks
if v1 > 117: v1 = 117 #ticks
#rospy.logwarn(v1)
message = 'j2 %d \r' % (v1) #% self._GetBaseAndExponents((v1)
rospy.logwarn("Sending right_arm_tilt_joint command: " + (message))
self._WriteSerial(message)
def _HandleJoint_3_Command(self, Command):
""" Handle movement requests.
left_arm_lift_joint
send message in degrees * 10
"""
v = Command.data # angel request in radians
self.left_lift = v
v1 = int(1023 - ((v + 1.82) * 195.3786081396)) #convert encoder value
if v1 < 400: v1 = 400 #degrees * 10
if v1 > 1000: v1 = 1000 #degrees * 10
message = 'j5 %d \r' % (v1) #% self._GetBaseAndExponents((v1)
rospy.logwarn("Sending left_arm_lift_joint command: " + (message))
self._WriteSerial(message)
#self._WriteSerial(message)
def _HandleJoint_4_Command(self, Command):
""" Handle movement requests.
right_arm_lift_joint
send message in degrees * 10
"""
v = Command.data # angel request in radians
self.right_lift = v
#v1 =int(1023 -((v + 1.05) * 195.3786081396))#convert encoder value
v1 = int((abs(v - 1.57) / 0.0042913) + 511)
if v1 < 520: v1 = 520 #degrees * 10
if v1 > 1000: v1 = 1000 #degrees * 10
rospy.logwarn("Handling lift command: " + str(v1))
#message = 's %.2f %.2f %.2f\r' % self._GetBaseAndExponents((v1))
message = 'j1 %d \r' % (v1) #% self._GetBaseAndExponents((v1)
rospy.logwarn("Sending new right_arm_lift_joint message: " + (message))
self._WriteSerial(message)
def _HandleJoint_8_Command(self, Command):
""" Handle movement requests.
left_arm_elbow_joint
send message in degrees * 10
"""
v = Command.data #
self.left_elbow = v
v1 = int((90 - degrees(v)) / 0.98) - 30
if v1 < 61: v1 = 61 #ticks
if v1 > 165: v1 = 167 #ticks
message = 'j8 %d \r' % (v1) #% self._GetBaseAndExponents((v1)
rospy.logwarn("Sending left_arm_elbow_joint command: " + (message))
self._WriteSerial(message)
def _GetBaseAndExponent(self, floatValue, resolution=4):
'''
Converts a float into a tuple holding two integers:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
if (floatValue == 0.0):
return (0, 0)
else:
exponent = int(1.0 + math.log10(abs(floatValue)))
multiplier = math.pow(10, resolution - exponent)
base = int(floatValue * multiplier)
return (base, exponent - resolution)
def _GetBaseAndExponents(self, floatValues, resolution=4):
'''
Converts a list or tuple of floats into a tuple holding two integers for each float:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
baseAndExponents = []
for floatValue in floatValues:
baseAndExponent = self._GetBaseAndExponent(floatValue)
baseAndExponents.append(baseAndExponent[0])
baseAndExponents.append(baseAndExponent[1])
return tuple(baseAndExponents)
def __init__(self, ):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
#port = rospy.get_param("~port", "/dev/ttyACM0")
#baud = int(rospy.get_param("~baud", "115200"))
#self.name = name
self.rate = rospy.get_param("~rate", 100.0)
self.fake = rospy.get_param("~sim", False)
self.cal_pan = rospy.get_param("~cal_pan", 0)
self.cal_tilt = rospy.get_param("~cal_tilt", 0)
self.cal_lift = rospy.get_param("~cal_lift", 0)
self.cal_rotate = rospy.get_param("~cal_rotate", 0)
self.cal_elbow = rospy.get_param("~cal_elbow", 0)
self.right_tilt = 0
self.left_tilt = 0
self.right_lift = 1.57
self.left_lift = 1.57
self.left_elbow = 0
#name = rospy.get_param("~name")
self._Counter = 0
rospy.init_node('r_shoulder')
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.logwarn("Starting Right shoulder with serial port: " + port +
", baud rate: " + str(baudRate))
# subscriptions
rospy.Subscriber('left_arm_tilt_joint/command', Float64,
self._HandleJoint_1_Command)
rospy.Subscriber('right_arm_tilt_joint/command', Float64,
self._HandleJoint_2_Command)
rospy.Subscriber('left_arm_lift_joint/command', Float64,
self._HandleJoint_3_Command)
rospy.Subscriber('right_arm_lift_joint/command', Float64,
self._HandleJoint_4_Command)
rospy.Subscriber('left_arm_elbow_joint/command', Float64,
self._HandleJoint_8_Command)
self._SerialPublisher = rospy.Publisher('arm_upper',
String,
queue_size=5)
self.P1_MotorPublisher = rospy.Publisher("/left_arm_tilt/motor_state",
MotorState,
queue_size=5)
self.P2_MotorPublisher = rospy.Publisher("/right_arm_tilt/motor_state",
MotorState,
queue_size=5)
self.P3_MotorPublisher = rospy.Publisher("/left_arm_lift/motor_state",
MotorState,
queue_size=5)
self.P4_MotorPublisher = rospy.Publisher("/right_arm_lift/motor_state",
MotorState,
queue_size=5)
self.P5_MotorPublisher = rospy.Publisher("/left_arm_elbow/motor_state",
MotorState,
queue_size=5)
self._P1_JointPublisher = rospy.Publisher("/left_arm_tilt_joint/state",
JointState,
queue_size=5)
self._P2_JointPublisher = rospy.Publisher(
"/right_arm_tilt_joint/state", JointState, queue_size=5)
self._P3_JointPublisher = rospy.Publisher("/left_arm_lift_joint/state",
JointState,
queue_size=5)
self._P4_JointPublisher = rospy.Publisher(
"/right_arm_lift_joint/state", JointState, queue_size=5)
self._P5_JointPublisher = rospy.Publisher(
"/left_arm_elbow_joint/state", JointState, queue_size=5)
self._right_lift_Publisher = rospy.Publisher("right_lift",
Float32,
queue_size=5)
self._right_tilt_Publisher = rospy.Publisher("right_tilt",
Float32,
queue_size=5)
self._left_lift_Publisher = rospy.Publisher("left_lift",
Float32,
queue_size=5)
self._left_tilt_Publisher = rospy.Publisher("left_tilt",
Float32,
queue_size=5)
self._left_elbow_Publisher = rospy.Publisher("left_elbow",
Float32,
queue_size=5)
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
class Launchpad_Class(object):
def __init__(self):
print "Iniciando clase launchpad"
######################################
#Variables de los sensores
self._Counter=0
self._reset_base1_value=0;
self._reset_shoulder1_value=0;
self._reset_elbow1_value=0;
self._base_encoder_value=0
self._base_wheel_speed=0
self._shoulder_encoder_value=0
self._shoulder_wheel_speed=0
self._elbow_encoder_value=0
self._elbow_wheel_speed=0
self._LastUpdate_Microsec=0
self._Second_Since_Last_Update=0
#########################################
#Asignamos valores del puerto y baudios de la stellaris
# port=rospy.get_param("~port","/dev/ttyACM0")
port=rospy.get_param("~port","/dev/ttyACM0")
baudRate=int(rospy.get_param("~baudRate",115200))
#########################################
rospy.loginfo("starting with serialport:"+port+"baudrate:"+str(baudRate))
self._SerialDataGateway=SerialDataGateway(port,baudRate,self._HandleReceivedLine)
rospy.loginfo("started serial communication")
###########################################
#publisher y suscribers
self._SerialPublisher=rospy.Publisher('serial_base',String,queue_size=10)
self.deltat=0
self.lastUpdate=0
self._Shoulder_Encoder=rospy.Publisher('shoulder_lec',Int16,queue_size=10)
self._Elbow_Encoder=rospy.Publisher('elbow_lec',Int16,queue_size=10)
self._Base1_Reset=rospy.Publisher('base1_reset',Int16,queue_size=10)
self._Shoulder1_Reset=rospy.Publisher('shoulder1_reset',Int16,queue_size=10)
self._Elbow1_Reset=rospy.Publisher('elbow1_reset',Int16,queue_size=10)
self._base_motor_speed=rospy.Subscriber('base_out',Int16,self._Update_Base_Speed)
self._shoulder_motor_speed=rospy.Subscriber('shoulder_out',Int16,self._Update_Shoulder_Speed)
self._elbow__motor_speed=rospy.Subscriber('elbow_out',Int16,self._Update_Elbow_Speed)
def _Update_Base_Speed(self,base_speed):
self._base_wheel_speed=base_speed.data
rospy.loginfo(base_speed.data)
speed_message='s %d %d %d \r' %(int(self._base_wheel_speed),int(self._shoulder_wheel_speed),int(self._elbow_wheel_speed))
self._WriteSerial(speed_message)
def _Update_Shoulder_Speed(self,shoulder_speed):
self._shoulder_wheel_speed=shoulder_speed.data
rospy.loginfo(shoulder_speed.data)
speed_message='s %d %d %d \r' %(int(self._base_wheel_speed),int(self._shoulder_wheel_speed),int(self._elbow_wheel_speed))
self._WriteSerial(speed_message)
def _Update_Elbow_Speed(self,elbow_speed):
self._elbow_wheel_speed=elbow_speed.data
rospy.loginfo(elbow_speed.data)
speed_message='s %d %d %d \r' %(int(self._base_wheel_speed),int(self._shoulder_wheel_speed),int(self._elbow_wheel_speed))
self._WriteSerial(speed_message)
def _HandleReceivedLine(self,line):
self._Counter=self._Counter+1
self._SerialPublisher.publish(String(str(self._Counter)+", in:"+line))
if(len(line)>0):
lineParts=line.split('\t')
try:
if(lineParts[0]=='e'):
self._shoulder_encoder_value=long(lineParts[1])
self._elbow_encoder_value=long(lineParts[2])
self._Shoulder_Encoder.publish(self._shoulder_encoder_value)
self._Elbow_Encoder.publish(self._elbow_encoder_value)
if(lineParts[0]=='n'):
self.reset_base1_value=int(lineParts[1])
self.reset_shoulder1_value=int(lineParts[2])
self.reset_elbow1_value=int(lineParts[3])
self._Base1_Reset.publish(self.reset_base1_value)
self._Shoulder1_Reset.publish(self.reset_shoulder1_value)
self._Elbow1_Reset.publish(self.reset_elbow1_value)
except:
rospy.logwarn("Error in sensor values")
rospy.logwarn(lineParts)
pass
def _WriteSerial(self,message):
self._SerialPublisher.publish(String(str(self._Counter)+", out:"+message))
self._SerialDataGateway.Write(message)
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
def Stop(self):
rospy.logdebug("Stoping")
self._SerialDataGateway.Stop()
def Reset_Launchpad(self):
print "reset"
reset='r\r'
self._WriteSerial(reset)
time.sleep(1)
self._WriteSerial(reset)
time.sleep(2)
def SubscribeSpeed(self):
a=1
def SendSpeed(self):
a=3
class Launchpad_Class(object):
def __init__(self):
print "Initializing Launchpad Class"
#######################################################################################################################
#Sensor variables
self._Counter = 0
self._left_encoder_value = 0
self._right_encoder_value = 0
self._battery_value = 0
self._ultrasonic_value = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
self._left_wheel_speed_ = 0
self._right_wheel_speed_ = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
#######################################################################################################################
#Get serial port and baud rate of Tiva C Launchpad
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
#Publisher for left and right wheel encoder values
self._Left_Encoder = rospy.Publisher('lwheel', Int64, queue_size=10)
self._Right_Encoder = rospy.Publisher('rwheel', Int64, queue_size=10)
#Publisher for Battery level(for upgrade purpose)
self._Battery_Level = rospy.Publisher('battery_level',
Float32,
queue_size=10)
#Publisher for Ultrasonic distance sensor
self._Ultrasonic_Value = rospy.Publisher('ultrasonic_distance',
Float32,
queue_size=10)
#Publisher for IMU rotation quaternion values
self._qx_ = rospy.Publisher('qx', Float32, queue_size=10)
self._qy_ = rospy.Publisher('qy', Float32, queue_size=10)
self._qz_ = rospy.Publisher('qz', Float32, queue_size=10)
self._qw_ = rospy.Publisher('qw', Float32, queue_size=10)
#Publisher for entire serial data
self._SerialPublisher = rospy.Publisher('serial',
String,
queue_size=10)
#######################################################################################################################
#Subscribers and Publishers of IMU data topic
self.frame_id = '/base_link'
self.cal_offset = 0.0
self.orientation = 0.0
self.cal_buffer = []
self.cal_buffer_length = 1000
self.imu_data = Imu(header=rospy.Header(frame_id="base_link"))
self.imu_data.orientation_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.angular_velocity_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.linear_acceleration_covariance = [
-1, 0, 0, 0, 0, 0, 0, 0, 0
]
self.gyro_measurement_range = 150.0
self.gyro_scale_correction = 1.35
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.deltat = 0
self.lastUpdate = 0
#New addon for computing quaternion
self.pi = 3.14159
self.GyroMeasError = float(self.pi * (40 / 180))
self.beta = float(math.sqrt(3 / 4) * self.GyroMeasError)
self.GyroMeasDrift = float(self.pi * (2 / 180))
self.zeta = float(math.sqrt(3 / 4) * self.GyroMeasDrift)
self.beta = math.sqrt(3 / 4) * self.GyroMeasError
self.q = [1, 0, 0, 0]
#######################################################################################################################
#Speed subscriber
self._left_motor_speed = rospy.Subscriber('left_wheel_speed', Float32,
self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_wheel_speed',
Float32,
self._Update_Right_Speed)
#######################################################################################################################
def _Update_Left_Speed(self, left_speed):
self._left_wheel_speed_ = left_speed.data
rospy.loginfo(left_speed.data)
speed_message = "s %d %d " % (int(
self._left_wheel_speed_), int(self._right_wheel_speed_))
self._WriteSerial(speed_message)
#######################################################################################################################################################3
def _Update_Right_Speed(self, right_speed):
self._right_wheel_speed_ = right_speed.data
rospy.loginfo(right_speed.data)
speed_message = "s %d %d " % (int(
self._left_wheel_speed_), int(self._right_wheel_speed_))
self._WriteSerial(speed_message)
#######################################################################################################################
#Calculate orientation from accelerometer and gyrometer
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(
String(str(self._Counter) + ", in: " + line))
if (len(line) > 0):
lineParts = line.split('\t')
try:
if (lineParts[0] == 'e'):
self._left_encoder_value = long(lineParts[1])
self._right_encoder_value = long(lineParts[2])
#######################################################################################################################
self._Left_Encoder.publish(self._left_encoder_value)
self._Right_Encoder.publish(self._right_encoder_value)
#######################################################################################################################
if (lineParts[0] == 'b'):
self._battery_value = float(lineParts[1])
#######################################################################################################################
self._Battery_Level.publish(self._battery_value)
#######################################################################################################################
if (lineParts[0] == 'u'):
self._ultrasonic_value = float(lineParts[1])
#######################################################################################################################
self._Ultrasonic_Value.publish(self._ultrasonic_value)
#######################################################################################################################
if (lineParts[0] == 'i'):
self._qx = float(lineParts[1])
self._qy = float(lineParts[2])
self._qz = float(lineParts[3])
self._qw = float(lineParts[4])
#######################################################################################################################
self._qx_.publish(self._qx)
self._qy_.publish(self._qy)
self._qz_.publish(self._qz)
self._qw_.publish(self._qw)
#######################################################################################################################
imu_msg = Imu()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = self.frame_id
imu_msg.header = h
imu_msg.orientation_covariance = (-1., ) * 9
imu_msg.angular_velocity_covariance = (-1., ) * 9
imu_msg.linear_acceleration_covariance = (-1., ) * 9
imu_msg.orientation.x = self._qx
imu_msg.orientation.y = self._qy
imu_msg.orientation.z = self._qz
imu_msg.orientation.w = self._qw
self.imu_pub.publish(imu_msg)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
#######################################################################################################################
def _WriteSerial(self, message):
self._SerialPublisher.publish(
String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
#######################################################################################################################
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
#######################################################################################################################
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
#######################################################################################################################
def Subscribe_Speed(self):
a = 1
# print "Subscribe speed"
#######################################################################################################################
def Reset_Launchpad(self):
print "Reset"
reset = 'r\r'
self._WriteSerial(reset)
time.sleep(1)
self._WriteSerial(reset)
time.sleep(2)
#######################################################################################################################
def Send_Speed(self):
# print "Set speed"
a = 3
def __init__(self):
print "Initializing Launchpad Class"
#######################################################################################################################
#Sensor variables
self._Counter = 0
self._left_encoder_value = 0
self._right_encoder_value = 0
self._battery_value = 0
self._ultrasonic_value = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
self._left_wheel_speed_ = 0
self._right_wheel_speed_ = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
#######################################################################################################################
#Get serial port and baud rate of Tiva C Launchpad
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
#Publisher for left and right wheel encoder values
self._Left_Encoder = rospy.Publisher('lwheel', Int64, queue_size=10)
self._Right_Encoder = rospy.Publisher('rwheel', Int64, queue_size=10)
#Publisher for Battery level(for upgrade purpose)
self._Battery_Level = rospy.Publisher('battery_level',
Float32,
queue_size=10)
#Publisher for Ultrasonic distance sensor
self._Ultrasonic_Value = rospy.Publisher('ultrasonic_distance',
Float32,
queue_size=10)
#Publisher for IMU rotation quaternion values
self._qx_ = rospy.Publisher('qx', Float32, queue_size=10)
self._qy_ = rospy.Publisher('qy', Float32, queue_size=10)
self._qz_ = rospy.Publisher('qz', Float32, queue_size=10)
self._qw_ = rospy.Publisher('qw', Float32, queue_size=10)
#Publisher for entire serial data
self._SerialPublisher = rospy.Publisher('serial',
String,
queue_size=10)
#######################################################################################################################
#Subscribers and Publishers of IMU data topic
self.frame_id = '/base_link'
self.cal_offset = 0.0
self.orientation = 0.0
self.cal_buffer = []
self.cal_buffer_length = 1000
self.imu_data = Imu(header=rospy.Header(frame_id="base_link"))
self.imu_data.orientation_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.angular_velocity_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.linear_acceleration_covariance = [
-1, 0, 0, 0, 0, 0, 0, 0, 0
]
self.gyro_measurement_range = 150.0
self.gyro_scale_correction = 1.35
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.deltat = 0
self.lastUpdate = 0
#New addon for computing quaternion
self.pi = 3.14159
self.GyroMeasError = float(self.pi * (40 / 180))
self.beta = float(math.sqrt(3 / 4) * self.GyroMeasError)
self.GyroMeasDrift = float(self.pi * (2 / 180))
self.zeta = float(math.sqrt(3 / 4) * self.GyroMeasDrift)
self.beta = math.sqrt(3 / 4) * self.GyroMeasError
self.q = [1, 0, 0, 0]
#######################################################################################################################
#Speed subscriber
self._left_motor_speed = rospy.Subscriber('left_wheel_speed', Float32,
self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_wheel_speed',
Float32,
self._Update_Right_Speed)
class Arduino(object):
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
# rospy.logdebug(str(self._Counter) + " " + line)
# if (self._Counter % 50 == 0):
self._SerialPublisher.publish(String(str(self._Counter) + " " + line))
if len(line) > 0:
lineParts = line.split('\t')
if lineParts[0] == 'o':
self._BroadcastOdometryInfo(lineParts)
return
if lineParts[0] == 'b':
self._BroadcastBatteryInfo(lineParts)
return
if lineParts[0] == 'd':
self._BroadcastAuto(lineParts)
return
if lineParts[0] == 'j':
self._BroadcastJoint(lineParts)
return
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
theta = float(lineParts[3])
vx = float(lineParts[4])
omega = float(lineParts[5])
# quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_footprint over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_footprint", "odom")
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_footprint"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
# rospy.loginfo(odometry)
except:
rospy.logwarn("Unexpected error odomfrom arduino.py :" +
str(sys.exc_info()[0]))
def _BroadcastBatteryInfo(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(partsCount)
if (partsCount < 1):
pass
try:
self.batteryVoltage = float(lineParts[1])
self._BatteryStatePublisher.publish(self.batteryVoltage)
rospy.loginfo(self.batteryVoltage)
except:
rospy.logwarn("Unexpected error battery:" + str(sys.exc_info()[0]))
def _BroadcastJoint(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
except:
rospy.logwarn("Unexpected error auto dock:" +
str(sys.exc_info()[0]))
def _BroadcastAuto(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(partsCount)
if partsCount < 1:
pass
try:
self.auto_left = int(lineParts[1])
self.auto_right = int(lineParts[2])
self.auto_bumper = int(lineParts[3])
self.docked = abs(self.auto_bumper - 1)
self.charged = round((self.docked * self.batteryVoltage), 2)
self._dock_state_Publisher.publish(self.charged)
except:
rospy.logwarn("Unexpected error auto dock:" +
str(sys.exc_info()[0]))
def _WriteSerial(self, message):
self._SerialPublisher.publish(
String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
def __init__(self, port="/dev/ttyUSB0", baudrate=115200):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
self._Counter = 0
rospy.init_node('driver_base')
port = rospy.get_param("~port", "/dev/ttyUSB0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.logwarn("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
rospy.Subscriber("auto_dock", String, self._AutoDock)
#group arm
rospy.Subscriber('left_joints', Left_joint, self.Left_arm)
rospy.Subscriber('right_joints', Right_joint, self.Right_arm)
# A service to manually start Auto dock
rospy.Service('start_auto_dock', AutoDock, self.Start_Auto_Dock)
self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
self._OdometryPublisher = rospy.Publisher("odom",
Odometry,
queue_size=5)
self._SerialPublisher = rospy.Publisher('serial', String, queue_size=5)
self._BatteryStatePublisher = rospy.Publisher("battery",
Float32,
queue_size=5)
self._dock_state_Publisher = rospy.Publisher("charge_state",
Float32,
queue_size=5)
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
def _HandleVelocityCommand(self, twistCommand):
#rospy.logwarn("recieved cmd")
""" Handle movement requests. """
x = twistCommand.linear.x # m/s
th = twistCommand.angular.z # rad/s
rospy.logwarn(th)
if x == 0:
# Turn in place
right = th * 0.43 / 2.0
left = -right
elif th == 0:
# Pure forward/backward motion
left = right = x
else:
# Rotation about a point in space
left = x - th * 0.43 / 2.0
right = x + th * 0.43 / 2.0
v_des_left = int(left * 16065 / 30) #
v_des_right = int(right * 16028 / 30) # ticks_per_meter 15915
message = 'm %d %d\r' % (v_des_left, v_des_right)
rospy.logwarn("Sending speed command message: " + message)
self._SerialDataGateway.Write(message)
#rospy.logwarn( v_des_left)
def _AutoDock(self, data):
# defunct remove after test
pass
def Start_Auto_Dock(self, data):
'''
start auto dock and charging sequance on the arduino
it will release when full
'''
rospy.logwarn("start auto dock service ")
return AutoDockResponse()
def Right_arm(self, data):
j0 = data.j0
j1 = data.j1
j2 = data.j2
j3 = data.j3
j4 = data.j4
j5 = data.j5
j6 = data.j6
#j7 = data.j7
message = 'r %d %d %d %d %d %d %d\r' % (j0, j1, j2, j3, j4, j5, j6)
rospy.logwarn("Sending speed command message: " + message)
self._SerialDataGateway.Write(message)
def Left_arm(self, data):
j0 = data.j0
j1 = data.j1
j2 = data.j2
j3 = data.j3
j4 = data.j4
j5 = data.j5
message = 'l %d %d %d %d %d %d\r' % (j0, j1, j2, j3, j4, j5)
rospy.logwarn("Sending speed command message: " + message)
self._SerialDataGateway.Write(message)
def __init__(self):
rospy.init_node('arlobot')
self.r = rospy.Rate(1) # 1hz refresh rate
self._Counter = 0 # For Propeller code's _HandleReceivedLine and _write_serial
self._motorsOn = False # Set to 1 if the motors are on, used with USB Relay Control board
self._SafeToOperate = False # Use arlobot_safety to set this
self._acPower = False # Track AC power status internally
self._unPlugging = False # Used for when arlobot_safety tells us to "UnPlug"!
self._wasUnplugging = False # Track previous unplugging status for motor control
self._SwitchingMotors = False # Prevent overlapping calls to _switch_motors
self._serialAvailable = False
self._serialTimeout = 0
self._leftMotorPower = False
self._rightMotorPower = False
self._laptop_battery_percent = 100
# Store last x, y and heading for reuse when we reset
# I took off off the ~, because that was causing these to reset to default on every restart
# even if roscore was still up.
self.lastX = rospy.get_param("lastX", 0.0)
self.lastY = rospy.get_param("lastY", 0.0)
self.lastHeading = rospy.get_param("lastHeading", 0.0)
self.alternate_heading = self.lastHeading
self.track_width = rospy.get_param("~driveGeometry/trackWidth", "0")
self.distance_per_count = rospy.get_param("~driveGeometry/distancePerCount", "0")
self.ignore_proximity = rospy.get_param("~ignoreProximity", False);
self.ignore_cliff_sensors = rospy.get_param("~ignoreCliffSensors", False);
self.robotParamChanged = False
# Get motor relay numbers for use later in _HandleUSBRelayStatus if USB Relay is in use:
self.relayExists = rospy.get_param("~usbRelayInstalled", False)
if self.relayExists:
# I think it is better to get these once than on every run of _HandleUSBRelayStatus
self.usbLeftMotorRelayLabel = rospy.get_param("~usbLeftMotorRelayLabel", "")
self.usbRightMotorRelayLabel = rospy.get_param("~usbRightMotorRelayLabel", "")
rospy.loginfo("Waiting for USB Relay find_relay service to start . . .")
rospy.wait_for_service('/arlobot_usbrelay/find_relay')
rospy.loginfo("USB Relay find_relay service started.")
try:
find_relay = rospy.ServiceProxy('/arlobot_usbrelay/find_relay', FindRelay)
self.leftMotorRelay = find_relay(self.usbLeftMotorRelayLabel)
self.rightMotorRelay = find_relay(self.usbRightMotorRelayLabel)
if self.leftMotorRelay.foundRelay and self.leftMotorRelay.foundRelay:
rospy.loginfo("Left = " + str(self.leftMotorRelay.relayNumber) + " & Right = " + str(
self.rightMotorRelay.relayNumber))
else:
self.relayExists = False
except rospy.ServiceException as e:
rospy.loginfo("Service call failed: %s" % e)
rospy.Subscriber("arlobot_usbrelay/usbRelayStatus", usbRelayStatus,
self._handle_usb_relay_status) # Safety Shutdown
# Subscriptions
rospy.Subscriber("cmd_vel", Twist, self._handle_velocity_command) # Is this line or the below bad redundancy?
rospy.Subscriber("arlobot_safety/safetyStatus", arloSafety, self._safety_shutdown) # Safety Shutdown
# Publishers
self._SerialPublisher = rospy.Publisher('serial', String, queue_size=10)
self._pirPublisher = rospy.Publisher('~pirState', Bool, queue_size=1) # for publishing PIR status
self._arlo_status_publisher = rospy.Publisher('arlo_status', arloStatus, queue_size=1)
# IF the Odometry Transform is done with the robot_pose_ekf do not publish it,
# but we are not using robot_pose_ekf, because it does nothing for us if you don't have a full IMU!
self._OdometryTransformBroadcaster = tf.TransformBroadcaster() # REMOVE this line if you use robot_pose_ekf
self._OdometryPublisher = rospy.Publisher("odom", Odometry, queue_size=10)
# We don't need to broadcast a transform, as it is static and contained within the URDF files
# self._SonarTransformBroadcaster = tf.TransformBroadcaster()
self._UltraSonicPublisher = rospy.Publisher("ultrasonic_scan", LaserScan, queue_size=10)
self._InfraredPublisher = rospy.Publisher("infrared_scan", LaserScan, queue_size=10)
# You can use the ~/metatron/scripts/find_propeller.sh script to find this, and
# You can set it by running this before starting this:
# rosparam set /arlobot/port $(~/metatron/scripts/find_propeller.sh)
port = rospy.get_param("~port", "/dev/ttyUSB1")
baud_rate = int(rospy.get_param("~baudRate", 115200))
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baud_rate))
self._SerialDataGateway = SerialDataGateway(port, baud_rate, self._handle_received_line)
self._OdomStationaryBroadcaster = OdomStationaryBroadcaster(self._broadcast_static_odometry_info)
class Arduino_Class(object):
def __init__(self):
print "Initializing Arduino Class"
#######################################################################################################################
#Sensor variables
self.req_right_ = 0
self.req_left_ = 0
self.actual_right = 0 # vector x
self.actual_left = 0 # vector y
self.time_ = 0.0 # time difference
self.gyro_X = 0.0
self.gyro_Y = 0.0
self.gyro_Z = 0.0
self.accel_X= 0.0
self.accel_Y= 0.0
self.accel_Z= 0.0
self._Counter=0
self.vector3 = Vector3Stamped()
#######################################################################################################################
#Get serial port and baud rate of Tiva C Arduino
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))
#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate, self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
self._Req_subscriber = rospy.Subscriber('rpm_req_msg',rpm,self._Handle_rpm_request)
self._Act_publisher = rospy.Publisher('rpm_act_msg',Vector3Stamped,queue_size = 10)
#self._Imu_publisher = rospy.Publisher('mpu9250_imu',Imu,queue_size=10)
#self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)
#######################################################################################################################
def _Handle_rpm_request(self, msg):
self.req_right_ = msg.req_right
self.req_left_ = msg.req_left
speed_message = '%d,%d,%d \r' %(int(1),int(self.req_right_),int(self.req_left_))
#speed_message = '%d,%d %d,%d \r' %(int(2),int(self.req_right_),int(3),int(self.req_left_))
self._WriteSerial(speed_message)
#######################################################################################################################
#Calculate orientation from accelerometer and gyrometer
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
#self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line))
#rospy.loginfo("hello1")
if(len(line) > 0):
#rospy.loginfo("hello2")
lineParts = line.split(',')
li = lineParts
ros_time = rospy.Time.now()
try:
#rospy.loginfo("Try")
if(li[0] == '5'):
self.actual_right = float(li[1])
self.actual_left = float(li[2])
self.time_ = float(li[3])
#rospy.loginfo(self.time_)
#rospy.loginfo("oK i got actual rpm")
self.vector3.header.stamp = ros_time
self.vector3.vector.x = self.actual_right
self.vector3.vector.y = self.actual_left
self.vector3.vector.z = self.time_
self._Act_publisher.publish(self.vector3)
# if(li[0] == '6'):
# self.gyro_X = float(li[1])
# self.gyro_Y = float(li[2])
# self.gyro_Z = float(li[3])
# self.acce_X = float(li[4])
# self.acce_Y = float(li[5])
# self.acce_Z = float(li[6])
# #rospy.loginfo("oK i got imu")
except:
#rospy.logwarn("Error in Sensor values")
rospy.loginfo("exception!")
pass
#######################################################################################################################
def _WriteSerial(self, message):
#self._SerialPublisher.publish(String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
#######################################################################################################################
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
#######################################################################################################################
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
def __init__(self):
print "Iniciando clase launchpad"
######################################
#Variables de los sensores
self._Counter = 0
self._reset_flipper1_value = 0
self._reset_flipper2_value = 0
self._reset_flipper3_value = 0
self._reset_flipper4_value = 0
self._left_encoder_value = 0
self._left_wheel_speed = 0
self._right_encoder_value = 0
self._right_wheel_speed = 0
self._flipper1_encoder_value = 0
self._flipper1_wheel_speed = 0
self._flipper2_encoder_value = 0
self._flipper2_wheel_speed = 0
self._flipper3_encoder_value = 0
self._flipper3_wheel_speed = 0
self._flipper4_encoder_value = 0
self._flipper4_wheel_speed = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
#########################################
#Asignamos valores del puerto y baudios de la stellaris
# port=rospy.get_param("~port","/dev/ttyACM0")
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#########################################
rospy.loginfo("starting with serialport:" + port + "baudrate:" +
str(baudRate))
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
rospy.loginfo("started serial communication")
###########################################
#publisher y suscribers
self._Left_Encoder = rospy.Publisher('left_lec', Int16, queue_size=10)
self._SerialPublisher = rospy.Publisher('serial_base',
String,
queue_size=10)
self.deltat = 0
self.lastUpdate = 0
self._Right_Encoder = rospy.Publisher('right_lec',
Int16,
queue_size=10)
##################################################################
##Publisher for flipper reset
self._Flipper1_Reset = rospy.Publisher('flipper1_reset',
Int16,
queue_size=10)
self._Flipper2_Reset = rospy.Publisher('flipper2_reset',
Int16,
queue_size=10)
self._Flipper3_Reset = rospy.Publisher('flipper3_reset',
Int16,
queue_size=10)
self._Flipper4_Reset = rospy.Publisher('flipper4_reset',
Int16,
queue_size=10)
##################################################################
##Publisher for encoders
self._Flipper1_Encoder = rospy.Publisher('flip1_lec',
Int16,
queue_size=10)
self._Flipper2_Encoder = rospy.Publisher('flip2_lec',
Int16,
queue_size=10)
self._Flipper3_Encoder = rospy.Publisher('flip3_lec',
Int16,
queue_size=10)
self._Flipper4_Encoder = rospy.Publisher('flip4_lec',
Int16,
queue_size=10)
##################################################################
##Publisher for IMU rotation quaternion values
self._qx = rospy.Publisher('qx', Float32, queue_size=10)
self._qy = rospy.Publisher('qy', Float32, queue_size=10)
self._qz = rospy.Publisher('qz', Float32, queue_size=10)
self._qw = rospy.Publisher('qw', Float32, queue_size=10)
self.frame_id = '/base_link'
self.cal_offset = 0.0
self.orientation = 0.0
self.cal_buffer = []
self.cal_buffer_length = 1000
self.imu_data = Imu(header=rospy.Header(frame_id="base_link"))
self.imu_data.orientation_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.angular_velocity_covariance = [
1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6
]
self.imu_data.linear_acceleration_covariance = [
-1, 0, 0, 0, 0, 0, 0, 0, 0
]
self.gyro_measurement_range = 150.0
self.gyro_scale_correction = 1.35
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.pi = 3.1416
self._left_motor_speed = rospy.Subscriber('left_out', Int16,
self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_out', Int16,
self._Update_Right_Speed)
self._flipper1_speed = rospy.Subscriber('flipper1_out', Int16,
self._Update_Flipper1_Speed)
self._flipper2_speed = rospy.Subscriber('flipper2_out', Int16,
self._Update_Flipper2_Speed)
self._flipper3_speed = rospy.Subscriber('flipper3_out', Int16,
self._Update_Flipper3_Speed)
self._flipper4_speed = rospy.Subscriber('flipper4_out', Int16,
self._Update_Flipper4_Speed)
class Arduino_Class(object):
def __init__(self):
print "Initializing Arduino Class"
#######################################################################################################################
#Sensor variables
self._Counter = 0
self._left_encoder_value = 0
self._right_encoder_value = 0
self._battery_value = 0
self._ultrasonic_value = 0
self._left_wheel_speed_ = 0
self._right_wheel_speed_ = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
#######################################################################################################################
#Get serial port and baud rate of Tiva C Launchpad
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
#Publisher for left and right wheel encoder values
self._Left_Encoder = rospy.Publisher('lwheel', Int64, queue_size=10)
self._Right_Encoder = rospy.Publisher('rwheel', Int64, queue_size=10)
#Publisher for Battery level(for upgrade purpose)
self._Battery_Level = rospy.Publisher('battery_level',
Float32,
queue_size=10)
#Publisher for Ultrasonic distance sensor
self._Ultrasonic_Value = rospy.Publisher('ultrasonic_distance',
Float32,
queue_size=10)
#Publisher for entire serial data
self._SerialPublisher = rospy.Publisher('serial',
String,
queue_size=10)
#######################################################################################################################
#Speed subscriber
self._left_motor_speed = rospy.Subscriber('left_wheel_speed', Float32,
self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_wheel_speed',
Float32,
self._Update_Right_Speed)
#######################################################################################################################
def _Update_Left_Speed(self, left_speed):
self._left_wheel_speed_ = left_speed.data
rospy.loginfo(left_speed.data)
speed_message = 's %d %d\r' % (int(
self._left_wheel_speed_), int(self._right_wheel_speed_))
self._WriteSerial(speed_message)
#######################################################################################################################################################3
def _Update_Right_Speed(self, right_speed):
self._right_wheel_speed_ = right_speed.data
rospy.loginfo(right_speed.data)
speed_message = 's %d %d\r' % (int(
self._left_wheel_speed_), int(self._right_wheel_speed_))
self._WriteSerial(speed_message)
#######################################################################################################################
#Calculate orientation from accelerometer and gyrometer
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
self._SerialPublisher.publish(
String(str(self._Counter) + ", in: " + line))
if (len(line) > 0):
lineParts = line.split('\t')
try:
if (lineParts[0] == 'e'):
self._left_encoder_value = long(lineParts[1])
self._right_encoder_value = long(lineParts[2])
#######################################################################################################################
self._Left_Encoder.publish(self._left_encoder_value)
self._Right_Encoder.publish(self._right_encoder_value)
#######################################################################################################################
if (lineParts[0] == 'b'):
self._battery_value = float(lineParts[1])
#######################################################################################################################
self._Battery_Level.publish(self._battery_value)
#######################################################################################################################
if (lineParts[0] == 'u'):
self._ultrasonic_value = float(lineParts[1])
#######################################################################################################################
self._Ultrasonic_Value.publish(self._ultrasonic_value)
except:
rospy.logwarn("Error in Sensor values")
rospy.logwarn(lineParts)
pass
#######################################################################################################################
def _WriteSerial(self, message):
self._SerialPublisher.publish(
String(str(self._Counter) + ", out: " + message))
self._SerialDataGateway.Write(message)
#######################################################################################################################
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
#######################################################################################################################
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
#######################################################################################################################
def Subscribe_Speed(self):
a = 1
# print "Subscribe speed"
#######################################################################################################################
def Reset_Arduino(self):
print "Reset"
reset = 'r\r'
self._WriteSerial(reset)
time.sleep(1)
self._WriteSerial(reset)
time.sleep(2)
#######################################################################################################################
def Send_Speed(self):
# print "Set speed"
a = 3
def __init__(self):
print "Initializing Arduino Class"
#######################################################################################################################
#Sensor variables
self._Counter = 0
self._left_encoder_value = 0
self._right_encoder_value = 0
self._battery_value = 0
self._ultrasonic_value = 0
self._left_wheel_speed_ = 0
self._right_wheel_speed_ = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
#######################################################################################################################
#Get serial port and baud rate of Tiva C Launchpad
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
#Publisher for left and right wheel encoder values
self._Left_Encoder = rospy.Publisher('lwheel', Int64, queue_size=10)
self._Right_Encoder = rospy.Publisher('rwheel', Int64, queue_size=10)
#Publisher for Battery level(for upgrade purpose)
self._Battery_Level = rospy.Publisher('battery_level',
Float32,
queue_size=10)
#Publisher for Ultrasonic distance sensor
self._Ultrasonic_Value = rospy.Publisher('ultrasonic_distance',
Float32,
queue_size=10)
#Publisher for entire serial data
self._SerialPublisher = rospy.Publisher('serial',
String,
queue_size=10)
#######################################################################################################################
#Speed subscriber
self._left_motor_speed = rospy.Subscriber('left_wheel_speed', Float32,
self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_wheel_speed',
Float32,
self._Update_Right_Speed)
class Arduino(object):
'''
Helper class for communicating with an Arduino board over serial port
'''
def _HandleReceivedLine(self, line):
self._Counter = self._Counter + 1
#rospy.logdebug(str(self._Counter) + " " + line)
#if (self._Counter % 50 == 0):
self._SerialPublisher.publish(String(str(self._Counter) + " " + line))
if (len(line) > 0):
lineParts = line.split('\t')
if (lineParts[0] == 'o'):
self._BroadcastOdometryInfo(lineParts)
return
if (lineParts[0] == 'b'):
#self._BroadcastBatteryInfo(lineParts)
return
if (lineParts[0] == "InitializeDriveGeometry"):
# controller requesting initialization
self._InitializeDriveGeometry()
return
if (lineParts[0] == "InitializeSpeedController"):
# controller requesting initialization
self._InitializeSpeedController()
return
if (lineParts[0] == "InitializeBatteryMonitor"):
# controller requesting initialization
self._InitializeBatteryMonitor()
return
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
theta = float(lineParts[3])
vx = float(lineParts[4])
omega = float(lineParts[5])
#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_link", "odom")
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
#rospy.loginfo(odometry)
except:
rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def _BroadcastBatteryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 1):
pass
try:
batteryVoltage = float(lineParts[1])
batteryState = BatteryState()
batteryState.voltage = batteryVoltage
if (batteryVoltage <= self._VoltageLowlimit):
batteryState.isLow = 1
if (batteryVoltage <= self._VoltageLowLowlimit):
batteryState.isLowLow = 1
self._BatteryStatePublisher.publish(batteryState)
#rospy.loginfo(batteryState)
except:
rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def __init__(self, port="/dev/ttyACM0", baudrate=115200):
'''
Initializes the receiver class.
port: The serial port to listen to.
baudrate: Baud rate for the serial communication
'''
self._Counter = 0
rospy.init_node('arduino')
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " +
str(baudRate))
# subscriptions
rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
self._SerialPublisher = rospy.Publisher('serial', String)
self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
self._OdometryPublisher = rospy.Publisher("odom", Odometry)
self._VoltageLowlimit = rospy.get_param(
"~batteryStateParams/voltageLowlimit", 12.0)
self._VoltageLowLowlimit = rospy.get_param(
"~batteryStateParams/voltageLowLowlimit", 11.7)
#self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
self._SetDriveGainsService = rospy.Service('setDriveControlGains',
SetDriveControlGains,
self._HandleSetDriveGains)
self._SerialDataGateway = SerialDataGateway(port, baudRate,
self._HandleReceivedLine)
def Start(self):
rospy.logdebug("Starting")
self._SerialDataGateway.Start()
def Stop(self):
rospy.logdebug("Stopping")
self._SerialDataGateway.Stop()
def _HandleVelocityCommand(self, twistCommand):
""" Handle movement requests. """
v = twistCommand.linear.x # m/s
omega = twistCommand.angular.z # rad/s
rospy.loginfo("Handling twist command: " + str(v) + "," + str(omega))
message = 's %d %d %d %d \r' % self._GetBaseAndExponents((v, omega))
rospy.logdebug("Sending speed command message: " + message)
self._SerialDataGateway.Write(message)
def _InitializeDriveGeometry(self):
wheelDiameter = rospy.get_param("~driveGeometry/wheelDiameter", 0.105)
trackWidth = rospy.get_param("~driveGeometry/trackWidth", 0.40)
countsPerRevolution = rospy.get_param(
"~driveGeometry/countsPerRevolution", 1600)
wheelDiameterParts = self._GetBaseAndExponent(wheelDiameter)
trackWidthParts = self._GetBaseAndExponent(trackWidth)
message = 'DriveGeometry %d %d %d %d %d\r' % (
wheelDiameterParts[0], wheelDiameterParts[1], trackWidthParts[0],
trackWidthParts[1], countsPerRevolution)
rospy.logdebug("Sending drive geometry params message: " + message)
self._SerialDataGateway.Write(message)
def _InitializeSpeedController(self):
velocityPParam = rospy.get_param("~speedController/velocityPParam",
1.0)
velocityIParam = rospy.get_param("~speedController/velocityIParam",
0.2)
turnPParam = rospy.get_param("~speedController/turnPParam", 0.01)
turnIParam = rospy.get_param("~speedController/turnIParam", 0.01)
commandTimeout = self._GetCommandTimeoutForSpeedController()
speedControllerParams = (velocityPParam, velocityIParam, turnPParam,
turnIParam, commandTimeout)
#rospy.loginfo(str(speedControllerParams))
self._WriteSpeedControllerParams(speedControllerParams)
def _GetCommandTimeoutForSpeedController(self):
"""
Returns the command timeout for the speed controller in seconds.
If no velocity command arrives for more than the specified timeout then the speed controller will stop the robot.
"""
return rospy.get_param("~speedController/commandTimeout", 0.5)
def _HandleSetDriveGains(self, request):
""" Handle the setting of the drive gains (PID). """
# We persist the new values in the parameter server
rospy.set_param(
"~speedController", {
'velocityPParam': request.velocityPParam,
'velocityPParam': request.velocityIParam,
'turnPParam': request.turnPParam,
'turnIParam': request.turnIParam
})
commandTimeout = self._GetCommandTimeoutForSpeedController()
speedControllerParams = (request.velocityPParam,
request.velocityIParam, request.turnPParam,
request.turnIParam, commandTimeout)
self._WriteSpeedControllerParams(speedControllerParams)
return SetDriveControlGainsResponse()
def _WriteSpeedControllerParams(self, speedControllerParams):
""" Writes the speed controller parameters (drive gains (PID), and command timeout) to the Arduino controller. """
rospy.logdebug(
"Handling '_WriteSpeedControllerParams'; received parameters " +
str(speedControllerParams))
message = 'SpeedControllerParams %d %d %d %d %d %d %d %d %d %d\r' % self._GetBaseAndExponents(
speedControllerParams)
rospy.logdebug("Sending differential drive gains message: " + message)
self._SerialDataGateway.Write(message)
def _InitializeBatteryMonitor(self):
rospy.logdebug("Initializing battery monitor. voltageTooLowLimit = " +
str(self._VoltageLowLowlimit))
message = 'BatteryMonitorParams %d %d\r' % self._GetBaseAndExponent(
self._VoltageLowLowlimit)
rospy.logdebug("Sending battery monitor params message: " + message)
self._SerialDataGateway.Write(message)
def _GetBaseAndExponent(self, floatValue, resolution=4):
'''
Converts a float into a tuple holding two integers:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
#floatValue=float(floatValue)
if (floatValue == 0.0):
return (0, 0)
else:
exponent = int(1.0 + math.log10(abs(floatValue)))
multiplier = math.pow(10, resolution - exponent)
base = int(floatValue * multiplier)
return (base, exponent - resolution)
def _GetBaseAndExponents(self, floatValues, resolution=4):
'''
Converts a list or tuple of floats into a tuple holding two integers for each float:
The base, an integer with the number of digits equaling resolution.
The exponent indicating what the base needs to multiplied with to get
back the original float value with the specified resolution.
'''
baseAndExponents = []
for floatValue in floatValues:
baseAndExponent = self._GetBaseAndExponent(floatValue)
baseAndExponents.append(baseAndExponent[0])
baseAndExponents.append(baseAndExponent[1])
return tuple(baseAndExponents)