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()