def __init__(self):
self.DepartmentName, self.PackageName = "ai", "scheduler"
rospy.init_node(self.PackageName, log_level=rospy.INFO)
self.AI = RobotAI()
self.AI.load_game_properties() # fetch available strategies and teams
self._hmi_init = False
self._ai_start_request = False
self._strat_publisher = rospy.Publisher(
"feedback/ard_hmi/set_strategies", SetStrategies, queue_size=10)
self._teams_publisher = rospy.Publisher("feedback/ard_hmi/set_teams",
SetTeams,
queue_size=10)
rospy.Subscriber("feedback/ard_hmi/hmi_event", HMIEvent,
self.on_hmi_event)
# Sending init status to ai/game_manager, subscribing to game_status status pub.
status_services = StatusServices(self.DepartmentName, self.PackageName,
self.on_arm)
status_services.ready(
True) # Tell ai/game_manager the node initialized successfuly.
r = rospy.Rate(5)
while not rospy.is_shutdown():
if not self._hmi_init:
self.send_game_properties()
if self._ai_start_request:
self.AI.start(AICommunication())
self._ai_start_request = False
r.sleep()
def startNode(self):
"""
Start the node and the clients.
"""
rospy.init_node (NODE_NAME, anonymous=False, log_level=rospy.INFO)
# Create the clients
self._pathfinderClient = PathfinderClient()
self._asservClient = AsservClient()
self._localizerClient = LocalizerClient()
self._collisionsClient = CollisionsClient(self._callbackEmergencyStop, self._callbackAsservResume)
self._mapClient = MapClient()
# Create action servers and topic publisher
self._actionSrv_Dogoto = actionlib.ActionServer(FULL_NODE_NAME + "/goto_action", DoGotoAction, self._handleDoGotoRequest, auto_start=False)
self._actionSrv_doGotoWaypoint = actionlib.ActionServer(FULL_NODE_NAME + "/gotowaypoint_action", DoGotoWaypointAction, self._handleDoGotoWaypointRequest, auto_start=False)
self._statusPublisher = rospy.Publisher(FULL_NODE_NAME + "/status", Status, queue_size=1)
# Launch the node
self._actionSrv_Dogoto.start()
self._actionSrv_doGotoWaypoint.start()
rospy.loginfo ("Ready to navigate!")
self._currentPlan = Plan(self._asservClient, self._pathfinderClient, self._planResultCallback, self._updateStatus)
self._updateStatus()
# Tell ai/game_manager the node initialized successfuly.
StatusServices(NODE_NAMESPACE, NODE_NAME).ready(True)
rospy.spin ()
def server():
rospy.init_node('definitions')
s = rospy.Service('memory/definitions/get', GetDefinition, callback)
rospy.logdebug("Definitions server ready")
# Tell ai/game_manager the node initialized successfuly.
StatusServices("memory", "definitions").ready(True)
rospy.spin()
def __init__(self):
super(BeltInterpreter, self).__init__()
rospy.init_node("belt_interpreter")
rospy.loginfo("Belt interpreter is initializing...")
# template for the sensor frame id, with '{}' being the sensor id
self.SENSOR_FRAME_ID = "belt_{}"
self.DEF_FILE = "processing/belt.xml"
self.TOPIC = "processing/belt_interpreter/rects"
self.SENSORS_TOPIC = "drivers/ard_others/belt_ranges"
self.PUB_RATE = rospy.Rate(10)
self.RECT_SCALE_WIDTH = 1.0
self.RECT_SCALE_HEIGHT = 1.0
self.WATCHDOG_PERIOD_BELT = rospy.Duration(0.015)
self.WATCHDOG_PERIOD_TERA = rospy.Duration(0.05)
self.PREVIOUS_DATA_SIZE = 2
filepath = self.fetch_definition()
self._belt_parser = BeltParser(filepath)
self._pub = rospy.Publisher(self.TOPIC, BeltRects, queue_size=1)
self._broadcaster = tf2_ros.StaticTransformBroadcaster()
self.pub_static_transforms()
self._sensors_sub = rospy.Subscriber(self.SENSORS_TOPIC, BeltRange,
self.callback)
self.syn_param_srv = Server(BeltInterpreterConfig, self.dyn_param_cb)
self._mutex = Lock()
self._watchdog = rospy.Timer(self.WATCHDOG_PERIOD_TERA, self.publish, oneshot=True)
self._current_rects = {}
self._current_statuses = {}
self._data_to_process = []
self._previous_rects = []
self._previous_statuses = []
self._same_bad_value_counter = {s: 0 for s in self._belt_parser.Sensors.keys()}
self._last_bad_value = {s: 0 for s in self._belt_parser.Sensors.keys()}
rospy.loginfo("Belt interpreter is ready. Listening for sensor data on '{}'.".format(self.SENSORS_TOPIC)) # TODO duplicate log with status_services.ready()
# Tell ai/game_manager the node initialized successfuly.
StatusServices("processing", "belt_interpreter").ready(True)
rospy.spin()
def __init__(self):
rospy.logdebug("Starting the node.")
# List containing the content of the xml definition file
self._components_list = []
# List containing the connected components and the corresponding port
self._connected_component_list = []
# List containing the final processing information, matching the serial port and the serial component
self._associated_port_list = []
# List of file descriptor for calls to rosserial
self._rosserial_call_list = []
# Init ROS stuff
rospy.init_node(NODE_NAME, anonymous=False, log_level=rospy.DEBUG)
def_dir = rospkg.RosPack().get_path(NODE_NAME) + "/def"
self._parse_xml_file(def_dir + "/components_list.xml")
self._parse_dmesg()
self._associate_port()
self._identify_arduino()
rospy.loginfo("Port_finder ready, found : " + str(self._associated_port_list))
self._srv_goto = rospy.Service("drivers/" + NODE_NAME + "/get_port", GetPort, self._callback_get_port)
# TODO find an other way ?
# Launch urg_node directly from port_finder to have the correct port
hokuyo_subprocess = None
hokuyo_port = self.get_port("hokuyo")
if hokuyo_port is not "":
hokuyo_subprocess = subprocess.Popen(["rosrun", "urg_node", "urg_node", "_serial_port:=" + hokuyo_port, "__ns:=sensors", "scan:=/processing/lidar_objects/scan"])
StatusServices("drivers", "port_finder").ready(True)
else:
rospy.logwarn("Couldn't start urg_node")
StatusServices("drivers", "port_finder").ready(False)
rospy.spin()
for rosserial_fd in self._rosserial_call_list:
rosserial_fd.terminate()
if hokuyo_subprocess:
hokuyo_subprocess.terminate()
def __init__(self):
super(Localizer, self).__init__()
rospy.init_node("localizer_node", anonymous=False)
self._br = tf2_ros.TransformBroadcaster()
# TODO : subscribe to all sources of info
self._sub_asserv = rospy.Subscriber(
"drivers/ard_asserv/pose2d", Pose2D, self.callback_asserv
)
self._data_asserv = None
# Tell ai/game_manager the node initialized successfuly.
StatusServices("recognition", "localizer").ready(True)
def __init__(self):
rospy.init_node("map", log_level=rospy.INFO)
rospy.logdebug("Started static_map node.")
map_manager.MapManager.load()
# Starting and publishing the table STL to RViz
self.markers = MarkersPublisher()
# Generate static occupancy images for pathfinder, etc.
occupancy = OccupancyGenerator()
# Starting service handlers (Get, Set, Transfer, GetOccupancy)
map_communication.MapServices(occupancy)
rospy.logdebug("[static_map] Map request servers ready.")
# Tell ai/game_manager the node initialized successfuly.
StatusServices("memory", "map").ready(True)
self.run()
def __init__(self):
rospy.init_node(NODE_NAME, anonymous=False, log_level=rospy.INFO)
teraranger_port = ""
self._node_subprocess = None
self._connected = False
try:
rospy.wait_for_service(GET_PORT_SERVICE_NAME,
GET_PORT_SERVICE_TIMEOUT)
self._src_client_get_port = rospy.ServiceProxy(
GET_PORT_SERVICE_NAME, GetPort)
teraranger_port = self._src_client_get_port("teraranger").port
except rospy.ROSException:
rospy.logerr(
"Port_finder service does not exist, can't fetch the teraranger port..."
)
status = StatusServices("drivers", "teraranger")
if teraranger_port != "":
self._node_subprocess = subprocess.Popen([
"rosrun", "teraranger", "one", "_portname:=" + teraranger_port,
"__ns:=/"
])
self._watchdog = rospy.Timer(rospy.Duration(WATCHDOG_PERIOD),
self._check_subprocess)
self._connected = True
status.ready(True)
else:
rospy.logerr(
"Teraranger port has not been found, start the node but can't send real data..."
)
status.ready(False)
self._range_value = 0.0
self._pub_belt_range = rospy.Publisher(
"drivers/ard_others/belt_ranges", BeltRange, queue_size=1)
self._sub_terarange = rospy.Subscriber(
"/teraranger_one", Range, self._callback_teranranger_range)
while not rospy.is_shutdown():
if self._connected:
self._pub_belt_range.publish(
BeltRange("sensor_tera1", self._range_value))
rospy.sleep(PUBLISH_INTERVAL)
if self._node_subprocess:
self._node_subprocess.terminate()
def __init__(self):
rospy.logdebug("[ASSERV] Starting asserv_node.")
# This dictionary stores the goals received by the DoGoto action and which are currently in processing
self._goals_dictionary = {}
# This dictionary stores the goals received by the Goto service and which are currently in processing
self._goto_srv_dictionary = {}
# Unique ID given to the received goal to manage it
self._goal_id_counter = 0
# Instance of the asserv object (simu or real)
self._asserv_instance = None
# Flag to know if the system has been halted (end of game)
self._is_halted = False
# Init ROS stuff
rospy.init_node(NODE_NAME, anonymous=False, log_level=rospy.INFO)
self._pub_robot_pose = rospy.Publisher(
"drivers/" + NODE_NAME + "/pose2d", Pose2D, queue_size=5
)
self._pub_robot_speed = rospy.Publisher(
"drivers/" + NODE_NAME + "/speed", RobotSpeed, queue_size=5
)
self._srv_goto = rospy.Service(
"drivers/" + NODE_NAME + "/goto", Goto, self._callback_goto
)
self._srv_pwm = rospy.Service(
"drivers/" + NODE_NAME + "/pwm", Pwm, self._callback_pwm
)
self._srv_speed = rospy.Service(
"drivers/" + NODE_NAME + "/speed", Speed, self._callback_speed
)
self._srv_set_pos = rospy.Service(
"drivers/" + NODE_NAME + "/set_pos", SetPos, self._callback_set_pos
)
self._srv_emergency_stop = rospy.Service(
"drivers/" + NODE_NAME + "/emergency_stop",
EmergencyStop,
self._callback_emergency_stop,
)
self._srv_params = rospy.Service(
"drivers/" + NODE_NAME + "/parameters",
Parameters,
self._callback_asserv_param,
)
self._srv_management = rospy.Service(
"drivers/" + NODE_NAME + "/management",
Management,
self._callback_management,
)
self._act_goto = actionlib.ActionServer(
"drivers/" + NODE_NAME + "/goto_action",
DoGotoAction,
self._callback_action_goto,
auto_start=False,
)
self._pub_tf_odom = tf2_ros.TransformBroadcaster()
self._act_goto.start()
self._srv_client_map_fill_waypoints = None
self._buffer_tf = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._buffer_tf)
try:
rospy.wait_for_service(GET_PORT_SERVICE_NAME, GET_PORT_SERVICE_TIMEOUT)
srv_client_get_port = rospy.ServiceProxy(GET_PORT_SERVICE_NAME, GetPort)
arduino_port = srv_client_get_port("ard_asserv").port
except rospy.ROSException as exc:
rospy.loginfo("Port_finder has not been launched...")
arduino_port = ""
rospy.loginfo("Port_finder returns value : " + arduino_port)
is_simu = False
if arduino_port == "":
rospy.logwarn("[ASSERV] Creation of the simu asserv.")
self._asserv_instance = asserv.AsservSimu(self)
is_simu = True
else:
rospy.loginfo("[ASSERV] Creation of the real asserv.")
self._asserv_instance = asserv.AsservReal(self, arduino_port)
try:
rospy.wait_for_service(GET_MAP_SERVICE_NAME, GET_MAP_SERVICE_TIMEOUT)
self._srv_client_map_fill_waypoints = rospy.ServiceProxy(
GET_MAP_SERVICE_NAME, MapGetWaypoint
)
rospy.logdebug("static_map has been found.")
except rospy.ROSException as exc:
rospy.logwarn("static_map has not been launched...")
# Tell ai/game_manager the node initialized successfuly.
StatusServices("drivers", "ard_asserv", None, self._callback_game_status).ready(
not is_simu
)
self._asserv_instance.start()