def __init__(self):
        
        self.start = False
        
        self.current = NED(0, 0, 0)
        self.position = None
        
        rospy.Subscriber('/scenario_start', Bool, self.start_cb)
        self.startPub = rospy.Publisher('start_sim', Bool, queue_size=1)

        self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
        
        rospy.Subscriber('position', NavSts, self.position_cb)
        rospy.Subscriber("ping_sensor", NED, self.ping_sensor_cb)
        rospy.Subscriber("current_sensor", TwistStamped, self.current_sensor_cb)
        
        self.cl = actionClient(rospy.get_namespace())
        
        rospy.spin()
    def __init__(self):
        
        self.start = False
        
        self.current = NED(0, 0, 0)
        self.position = None
        
        self.neighbors = {}
        rospy.Subscriber('/hello', String, self.hello_cb)
        self.helloPub = rospy.Publisher('/hello', String, queue_size=100)
        
        rospy.Subscriber('/scenario_start', Bool, self.start_cb)
        self.startPub = rospy.Publisher('start_sim', Bool, queue_size=1)

        self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1)
        
        rospy.Subscriber('position', NavSts, self.position_cb)

        self.batteryLevel = None
        #rospy.Subscriber('battery_level', Int32, self.battery_level_cb)
        self.batteryLevel = 80
        rospy.Service('charge_info', GetChargeInfo, self.charge_info_srv)
        self.chargeType = 'charger'
        
        self.chargeRequests = []
        rospy.Subscriber('/battery_alert', NavSts, self.charge_cb)
        
        self.cl = actionClient(rospy.get_namespace())
        self.mdvrp = mdvrp.mdvrp()
        self.cbmAlgorithm = cbm_algorithm.cbm_algorithm("cbm_parameters.xml")
        
        self.bestSolutionPub = rospy.Publisher("/bestSolution", BestSolution, queue_size=20)
        self.weightMatrixPub = rospy.Publisher("/weightMatrix", WeightMatrix, queue_size=20)
        rospy.Subscriber('/optimize', Bool, self.charging_procedure)
        
        rospy.spin()