def check_heartbeat(self):
conf.log("checking for unresponsive machines")
dead_machines = []
for machineid in self._node_response:
if rospy.get_time() - self._node_response[machineid] > 5:
dead_machines.append(machineid)
for machineid in dead_machines:
conf.log("Did not receive heartbeat from {0}. Disconnecting".format(machineid))
self._node_response.pop(machineid)
self._connected_nodes.pop(machineid)
react.db.del_machine(react.db.machine(machineid))
def __call__(cls, *args, **kwargs):
"""
Intercept constructor calls and register created instances with react.db
"""
obj = super(ReactObjMClass, cls).__call__(*args, **kwargs)
if type(obj).is_record():
if react.curr_node is not None:
m = react.curr_node.machine()
if m is not None:
obj._id = "%s__%s" % (m.id(), obj._id)
conf.log("!!! changed id: %s" % obj)
react.db.add(cls.kind(), obj)
return obj
def every_100ms(self):
for car in self.cars:
conf.log("-------------- car: %d, %d" % (car.data.pos_x, car.data.pos_y))
self.draw_cars()
for car in self.cars:
closeCars = []
for otherCar in self.cars:
if abs(car.data.pos_x - otherCar.data.pos_x) <= 5:
if abs(car.data.pos_y - otherCar.data.pos_y) <= 5:
closeCars.append(otherCar)
self.trigger(UpdateSensor(sender=self, receiver=car, cars=closeCars))
# this looks wrong: the receiver is not set!
self.trigger(UpdateRemote(cars = self.cars))
def check_heartbeat(self):
conf.log("checking for unresponsive machines")
dead_machines = []
for machineid in self._node_response:
if rospy.get_time() - self._node_response[machineid] > 5:
dead_machines.append(machineid)
for machineid in dead_machines:
conf.log(
"Did not receive heartbeat from {0}. Disconnecting".format(
machineid))
self._node_response.pop(machineid)
self._connected_nodes.pop(machineid)
react.db.del_machine(react.db.machine(machineid))
def __call__(cls, *args, **kwargs):
"""
Intercept constructor calls and register created instances with react.db
"""
obj = super(ReactObjMClass, cls).__call__(*args, **kwargs)
if type(obj).is_record():
if react.curr_node is not None:
m = react.curr_node.machine()
if m is not None:
obj._id = "%s__%s" % (m.id(), obj._id)
conf.log("!!! changed id: %s" % obj)
react.db.add(cls.kind(), obj)
return obj
def forward_event_req(self, req, ev=None):
if ev is None:
ev = ser.deserialize_objval(req.event)
my_machine_id = -1
if self.machine() is not None:
my_machine_id = self.machine().id()
ev_receiver = ev.get_receiver()
if ev_receiver.id() != my_machine_id:
#TODO: this can fail
conf.log("forwarding event %s to %s", ev, ev_receiver)
ev_srv = rospy.ServiceProxy(event_srv_name(ev_receiver),
react.srv.EventSrv)
ev_srv(req.event)
def forward_event_req(self, req, ev=None):
if ev is None:
ev = ser.deserialize_objval(req.event)
my_machine_id = -1
if self.machine() is not None:
my_machine_id = self.machine().id()
ev_receiver = ev.get_receiver()
if ev_receiver.id() != my_machine_id:
#TODO: this can fail
conf.log("forwarding event %s to %s", ev, ev_receiver)
ev_srv = rospy.ServiceProxy(event_srv_name(ev_receiver),
react.srv.EventSrv)
ev_srv(req.event)
def unreg_handler(self, req):
"""
Handler for the UnregisterMachineSrv service.
"""
mid = req.machine.obj_id
machine = self._connected_nodes.pop(mid, None)
self._node_response.pop(mid)
if machine is None:
conf.warn("Machine %s not found in the list of connected nodes" %
req.machine)
else:
conf.log("Machine %s disconnected" % machine)
machine.delete()
resp = {"status": "ok"}
return react.srv.UnregisterMachineSrvResponse(**resp)
def every_100ms(self):
for car in self.cars:
conf.log("-------------- car: %d, %d" %
(car.data.pos_x, car.data.pos_y))
self.draw_cars()
for car in self.cars:
closeCars = []
for otherCar in self.cars:
if abs(car.data.pos_x - otherCar.data.pos_x) <= 5:
if abs(car.data.pos_y - otherCar.data.pos_y) <= 5:
closeCars.append(otherCar)
self.trigger(
UpdateSensor(sender=self, receiver=car, cars=closeCars))
# this looks wrong: the receiver is not set!
self.trigger(UpdateRemote(cars=self.cars))
def unreg_handler(self, req):
"""
Handler for the UnregisterMachineSrv service.
"""
mid = req.machine.obj_id
machine = self._connected_nodes.pop(mid, None)
self._node_response.pop(mid)
if machine is None:
conf.warn("Machine %s not found in the list of connected nodes" % req.machine)
else:
conf.log("Machine %s disconnected" % machine)
machine.delete()
resp = {
"status": "ok"
}
return react.srv.UnregisterMachineSrvResponse(**resp)
def _register_node(self):
"""
Registers this node with ReactCore via the RegisterMachineSrv
service. Upon successful registration, it initializes a ROS
node with the name received from ReactCore, and starts its own
PushSrv service with the same name.
"""
mname = self.machine_name()
mcls = react.meta.machine(mname)
if mcls is None:
conf.fatal("Machine class %s not found", mname)
if hasattr(mcls.cls(), "react_config"):
getattr(mcls.cls(), "react_config")();
conf.log("Requesting machine registration for machine %s", mname)
rospy.wait_for_service(react.core.REG_SRV_NAME)
reg_srv = rospy.ServiceProxy(react.core.REG_SRV_NAME, react.srv.RegisterMachineSrv)
ans = reg_srv(mname)
conf.debug("Received response: %s", ans)
self._machine = ser.deserialize_objref(ans.this_machine)
self._node_name = ans.this_node_name
self._update_other_machines(ans.other_machines)
def _register_node(self):
"""
Registers this node with ReactCore via the RegisterMachineSrv
service. Upon successful registration, it initializes a ROS
node with the name received from ReactCore, and starts its own
PushSrv service with the same name.
"""
mname = self.machine_name()
mcls = react.meta.machine(mname)
if mcls is None:
conf.fatal("Machine class %s not found", mname)
if hasattr(mcls.cls(), "react_config"):
getattr(mcls.cls(), "react_config")()
conf.log("Requesting machine registration for machine %s", mname)
rospy.wait_for_service(react.core.REG_SRV_NAME)
reg_srv = rospy.ServiceProxy(react.core.REG_SRV_NAME,
react.srv.RegisterMachineSrv)
ans = reg_srv(mname)
conf.debug("Received response: %s", ans)
self._machine = ser.deserialize_objref(ans.this_machine)
self._node_name = ans.this_node_name
self._update_other_machines(ans.other_machines)
def start_machine(self):
"""
(1) registers this machien with ReactCore (by calling
self._register_node()). That will initialize
a ROS node, and also start some services to allow
ReactCore to talk to this node directly.
"""
try:
self._register_node()
rospy.init_node(self.node_name())
if conf.heartbeat:
conf.log("scheduling periodic hearbeat")
self._scheduler.every(1, self._send_heartbeat)
conf.log("initializing push service")
rospy.Service(self.my_push_srv_name(), react.srv.PushSrv,
self.push_handler)
conf.log("initializing events service ...")
rospy.Service(self.my_event_srv_name(), react.srv.EventSrv,
self.get_srv_handler("event", self.event_handler))
sys.exitfunc = self._on_exit
if hasattr(self.machine(), "on_start"):
self.machine().on_start()
for every_spec in self.machine().meta().timer_events():
self._scheduler.every(
every_spec[1],
curry(self._trigger_machine_method, every_spec[0]))
in_thread(self.cli_thr_func, conf.cli)
in_thread(rospy.spin, conf.rospy_spin)
except rospy.ServiceException, e:
conf.error("Service call failed: %s", e)
def start_machine(self):
"""
(1) registers this machien with ReactCore (by calling
self._register_node()). That will initialize
a ROS node, and also start some services to allow
ReactCore to talk to this node directly.
"""
try:
self._register_node()
rospy.init_node(self.node_name())
if conf.heartbeat:
conf.log("scheduling periodic hearbeat")
self._scheduler.every(1, self._send_heartbeat)
conf.log("initializing push service")
rospy.Service(self.my_push_srv_name(), react.srv.PushSrv, self.push_handler)
conf.log("initializing events service ...")
rospy.Service(self.my_event_srv_name(),
react.srv.EventSrv,
self.get_srv_handler("event", self.event_handler))
sys.exitfunc = self._on_exit
if hasattr(self.machine(), "on_start"):
self.machine().on_start()
for every_spec in self.machine().meta().timer_events():
self._scheduler.every(every_spec[1],
curry(self._trigger_machine_method, every_spec[0]))
in_thread(self.cli_thr_func, conf.cli)
in_thread(rospy.spin, conf.rospy_spin)
except rospy.ServiceException, e:
conf.error("Service call failed: %s", e)
def start_core(self):
rospy.init_node('reactcore')
conf.log("initializing registration service ...")
rospy.Service(react.core.REG_SRV_NAME,
react.srv.RegisterMachineSrv,
self.get_srv_handler("registration", self.reg_handler))
conf.log("initializing unregistration service ...")
rospy.Service(react.core.UNREG_SRV_NAME,
react.srv.UnregisterMachineSrv,
self.get_srv_handler("unregistration", self.unreg_handler))
conf.log("initializing events service ...")
rospy.Service(react.core.EVENT_SRV_NAME,
react.srv.EventSrv,
self.get_srv_handler("event", self.event_handler))
conf.log("initializing node discovery service ...")
rospy.Service(react.core.NODE_DISCOVERY_SRV_NAME,
react.srv.NodeDiscoverySrv,
self.get_srv_handler("discover", self.node_discovery_handler))
conf.log("initializing heartbeat service ...")
rospy.Service(react.core.HEARTBEAT_SRV_NAME,
react.srv.HeartbeatSrv,
self.get_srv_handler("heartbeat", self.heartbeat_handler, False))
if conf.heartbeat:
conf.log("scheduling check heartbeat ...")
self._scheduler.every(5, self.check_heartbeat)
try:
in_thread(self.cli_thr_func, conf.cli)
in_thread(rospy.spin, conf.rospy_spin)
except:
conf.error("Error: unable to start thread")
def start_core(self):
rospy.init_node('reactcore')
conf.log("initializing registration service ...")
rospy.Service(react.core.REG_SRV_NAME, react.srv.RegisterMachineSrv,
self.get_srv_handler("registration", self.reg_handler))
conf.log("initializing unregistration service ...")
rospy.Service(
react.core.UNREG_SRV_NAME, react.srv.UnregisterMachineSrv,
self.get_srv_handler("unregistration", self.unreg_handler))
conf.log("initializing events service ...")
rospy.Service(react.core.EVENT_SRV_NAME, react.srv.EventSrv,
self.get_srv_handler("event", self.event_handler))
conf.log("initializing node discovery service ...")
rospy.Service(
react.core.NODE_DISCOVERY_SRV_NAME, react.srv.NodeDiscoverySrv,
self.get_srv_handler("discover", self.node_discovery_handler))
conf.log("initializing heartbeat service ...")
rospy.Service(
react.core.HEARTBEAT_SRV_NAME, react.srv.HeartbeatSrv,
self.get_srv_handler("heartbeat", self.heartbeat_handler, False))
if conf.heartbeat:
conf.log("scheduling check heartbeat ...")
self._scheduler.every(5, self.check_heartbeat)
try:
in_thread(self.cli_thr_func, conf.cli)
in_thread(rospy.spin, conf.rospy_spin)
except:
conf.error("Error: unable to start thread")