def __init__(self):
uid = str(os.getpid())
try:
print "Initialize: serial_converter_{0}".format(uid)
rospy.init_node("serial_converter_{0}".format(uid),
log_level=rospy.WARN)
except:
print "Node already initialized: {0}".format(rospy.get_caller_id())
rospy.loginfo("ROS Serial Python Node '{0}'".format(uid))
self.__odomPublisher = rospy.Publisher('/emerald_ai/serial/odom',
Odometry,
queue_size=10)
self.__wheelDiameter = HardwareConfig().GetInt("Wheel",
"Diameter") # in mm
self.__wheelBaseline = HardwareConfig().GetInt("Wheel",
"BaseWidth") # in mm
self.__encoderTicksPerRevelation = HardwareConfig().GetInt(
"Wheel", "EncoderTicksPerRevelation")
self.__wheelDistancePerTickLeft = math.pi * self.__wheelDiameter / self.__encoderTicksPerRevelation
self.__wheelDistancePerTickRight = math.pi * self.__wheelDiameter / self.__encoderTicksPerRevelation
# start position
self.__x = 0.0
self.__y = 0.0
self.__th = 0.0
self.__currentTime = rospy.Time.now()
self.__lastTime = rospy.Time.now()
def __init__(self, topic="/cmd_vel"):
uid = str(os.getpid())
try:
print "Initialize: serial_converter_{0}".format(uid)
rospy.init_node("serial_converter_{0}".format(uid),
log_level=rospy.WARN)
except:
print "Node already initialized: {0}".format(rospy.get_caller_id())
rospy.loginfo("ROS Serial Python Node '{0}'".format(uid))
self.__left = 0
self.__right = 0
self.__timeoutTicks = HardwareConfig().GetInt("Wheel.PID",
"TimeoutTicks")
self.__ticksSinceLastTwistInstruction = self.__timeoutTicks
self.__wheelBaseInMM = HardwareConfig().GetInt("Wheel",
"BaseWidth") # in mm
self.__inPlaceRoatation = HardwareConfig().GetBoolean(
"Wheel", "InPlaceRoatation")
self.__currentTime = rospy.Time.now()
self.__lastTime = rospy.Time.now()
self.__leftPidController = PIDController(rospy.Time.now())
self.__rightPidController = PIDController(rospy.Time.now())
rospy.Subscriber(topic, Twist, self.__twistCallback)
self.__serialPublisher = rospy.Publisher(
'/emerald_ai/serial{0}/raw_serial'.format(topic),
String,
queue_size=3)
def __init__(self):
uid = str(os.getpid())
try:
print "Initialize: serial_converter_{0}".format(uid)
rospy.init_node("serial_converter_{0}".format(uid),
log_level=rospy.WARN)
except:
print "Node already initialized: {0}".format(rospy.get_caller_id())
rospy.loginfo("ROS Serial Python Node '{0}'".format(uid))
self.__laserPublisherOne = rospy.Publisher(
'/emerald_ai/serial/radar/laser/one', LaserScan, queue_size=20)
self.__laserPublisherTwo = rospy.Publisher(
'/emerald_ai/serial/radar/laser/two', LaserScan, queue_size=20)
self.__laserPublisherThree = rospy.Publisher(
'/emerald_ai/serial/radar/laser/three', LaserScan, queue_size=20)
self.__laserPublisherFour = rospy.Publisher(
'/emerald_ai/serial/radar/laser/four', LaserScan, queue_size=20)
self.__laserMessage = LaserScan()
self.__laserMessage.angle_min = HardwareConfig().GetFloat(
"Laser.SinglePoint", "AngleMin")
self.__laserMessage.angle_max = HardwareConfig().GetFloat(
"Laser.SinglePoint", "AngleMax")
self.__laserMessage.angle_increment = HardwareConfig().GetFloat(
"Laser.SinglePoint", "AngleIncrement")
self.__laserMessage.time_increment = HardwareConfig().GetFloat(
"Laser.SinglePoint", "TimeIncrement")
self.__laserMessage.range_min = round(
HardwareConfig().GetFloat("Laser.SinglePoint", "RangeMin") / 100.0,
2)
self.__laserMessage.range_max = round(
HardwareConfig().GetFloat("Laser.SinglePoint", "RangeMax") / 100.0,
2)
def __init__(self):
uid = str(os.getpid())
try:
print "Initialize: serial_converter_{0}".format(uid)
rospy.init_node("serial_converter_{0}".format(uid),
log_level=rospy.WARN)
except:
print "Node already initialized: {0}".format(rospy.get_caller_id())
rospy.loginfo("ROS Serial Python Node '{0}'".format(uid))
self.__rangePublisherFront = rospy.Publisher(
'/emerald_ai/serial/radar/range/front', Range, queue_size=10)
self.__rangePublisherBack = rospy.Publisher(
'/emerald_ai/serial/radar/range/back', Range, queue_size=10)
self.__rangeMessage = Range()
self.__rangeMessage.radiation_type = 0
self.__rangeMessage.min_range = HardwareConfig().GetFloat(
"Ultrasonic", "RangeMin") / 100.0
self.__rangeMessage.max_range = HardwareConfig().GetFloat(
"Ultrasonic", "RangeMax") / 100.0
self.__rangeMessage.field_of_view = HardwareConfig().GetFloat(
"Ultrasonic", "FieldOfView")
self.__rangeMessage.radiation_type = 0
def __init__(self):
uid = str(os.getpid())
try:
print "Initialize: serial_converter_{0}".format(uid)
rospy.init_node("serial_converter_{0}".format(uid),
log_level=rospy.WARN)
except:
print "Node already initialized: {0}".format(rospy.get_caller_id())
rospy.loginfo("ROS Serial Python Node '{0}'".format(uid))
self.__laserPublisher360One = rospy.Publisher(
'/emerald_ai/serial/radar/laser/360/one', LaserScan, queue_size=20)
self.__laserPublisher360Two = rospy.Publisher(
'/emerald_ai/serial/radar/laser/360/two', LaserScan, queue_size=20)
self.__laserMessage = LaserScan()
self.__laserMessage.range_min = round(
HardwareConfig().GetFloat("Laser.FullRange", "RangeMin") / 100.0,
2)
self.__laserMessage.range_max = round(
HardwareConfig().GetFloat("Laser.FullRange", "RangeMax") / 100.0,
2)
self.__laserScannerCount = HardwareConfig().GetInt(
"Laser.FullRange", "LaserScannerCount")
self.__laserScannerCorrectionFast = HardwareConfig().GetInt(
"Laser.FullRange", "LaserScannerCorrectionFast")
self.__laserScannerCorrectionMedium = HardwareConfig().GetInt(
"Laser.FullRange", "LaserScannerCorrectionMedium")
self.__laserScannerCorrectionDetailed = HardwareConfig().GetInt(
"Laser.FullRange", "LaserScannerCorrectionDetailed")
self.__laserDictionary = {}
self.__laserDictionary["one"] = {}
self.__laserDictionary["two"] = {}
self.__laserScanStartTime = {}
self.__laserScanStartTime["one"] = time.time()
self.__laserScanStartTime["two"] = time.time()
self.__laserScanStartAngle = {}
self.__laserScanStartAngle["one"] = None
self.__laserScanStartAngle["two"] = None
def __init__(self, timeNow):
### initialize variables
self.target = 0
self.motor = 0
self.vel = 0
self.integral = 0
self.error = 0
self.derivative = 0
self.previousError = 0
self.wheelPrev = 0
self.wheelLatest = 0
self.then = timeNow
self.wheelMult = 0
self.prevEncoder = 0
### get parameters ####
self.BaseSpeedFactor = HardwareConfig().GetFloat("Wheel.PID", "BaseSpeedFactor")
self.VelocityMin = HardwareConfig().GetFloat("Wheel.PID", "VelocityMin")
self.VelocityMax = HardwareConfig().GetFloat("Wheel.PID", "VelocityMax")
self.VelocityApproachMin = HardwareConfig().GetFloat("Wheel.PID", "VelocityApproachMin")
self.VelocityApproachValue = HardwareConfig().GetFloat("Wheel.PID", "VelocityApproachValue")
self.Kp = HardwareConfig().GetFloat("Wheel.PID", "Kp")
self.Ki = HardwareConfig().GetFloat("Wheel.PID", "Ki")
self.Kd = HardwareConfig().GetFloat("Wheel.PID", "Kd")
self.outMin = HardwareConfig().GetFloat("Wheel.PID", "OutMin")
self.outMax = HardwareConfig().GetFloat("Wheel.PID", "OutMax")
self.rollingPts = HardwareConfig().GetInt("Wheel.PID", "RollingPts")
self.timeoutTicks = HardwareConfig().GetInt("Wheel.PID", "TimeoutTicks")
self.ticksPerMeter = HardwareConfig().GetInt("Wheel", "EncoderTicksPerMeter")
self.velThreshold = HardwareConfig().GetFloat("Wheel.PID", "VelThreshold")
self.encoderMin = HardwareConfig().GetFloat("Wheel.PID", "EncoderMin")
self.encoderMax = HardwareConfig().GetFloat("Wheel.PID", "EncoderMax")
self.encoderLowWrap = (self.encoderMax - self.encoderMin) * 0.3 + self.encoderMin
self.encoderHighWrap = (self.encoderMax - self.encoderMin) * 0.7 + self.encoderMin
self.prevVel = [0.0] * self.rollingPts
self.wheelLatest = 0.0
self.prevPidTime = timeNow
self.ticksSinceLastTarget = self.timeoutTicks
import multiprocessing
import time
import signal
from EmeraldAI.Logic.Modules import Pid
from EmeraldAI.Logic.ROS.Serial.SerialFinder import SerialFinder
from EmeraldAI.Logic.ROS.Serial.SerialConnector import SerialConnector
from EmeraldAI.Logic.ROS.Serial.SerialWheelToOdometry import SerialWheelToOdometry
from EmeraldAI.Logic.ROS.Serial.TwistToSerialWheel import TwistToSerialWheel
from EmeraldAI.Logic.ROS.Serial.SerialRadarToRange import SerialRadarToRange
from EmeraldAI.Logic.ROS.Serial.SerialLaserToLaser import SerialLaserToLaser
from EmeraldAI.Logic.ROS.Serial.SerialLaserToLaser360 import SerialLaserToLaser360
from EmeraldAI.Logic.ROS.Serial.SerialImuToImu import SerialImuToImu
from EmeraldAI.Config.HardwareConfig import HardwareConfig
UseUltrasonic = HardwareConfig().GetBoolean("Sensor", "UseUltrasonic")
UseLaser = HardwareConfig().GetBoolean("Sensor", "UseLaser")
def ProcessHandler(signal, frame):
sys.exit(0)
def Processing(port, baud):
serialConnect = SerialConnector(port, baud)
imuToImu = SerialImuToImu()
isImu = True
radarToRange = SerialRadarToRange()
isRadarRange = True
laserToLaser = SerialLaserToLaser()