Finite state machine Node

Definiton:

Breifly, finite_state_machine node define 5 task-level architecture for state machine in order to make robot abile to:

1. SETUP_MAP: Detecte marker ID’s use my_moveit.cpp node to move manipulator and add detected room’s id into the list of the rooms
2. GO_IN_ROOM: Patrolling the robot in map autonomously using move_base package and Finding target room via ontology map and control battery level using helper script set_object_state.py.
3. INSPECT_ROOM: Udate ontology map using helper script build_ontology_map.py and inspect room by camera on manipulator
4. GO_TO_CHARGER: In case battery level lower than thershold
5. WAIT_FOR_CHARGING: After a while machine state changes to the GO_IN_ROOM.

Subscribes to:

/image_id —> to get rooms information

Services:

/room_info —> uses RoomInformation.srv

/state/get_pose —> uses GetPose.srv

SimpleActionClient:

/move_base —> uses MoveBaseAction

class scripts.finite_state_machine.GO_IN_ROOM

GO_IN_ROOM state is useful for moving the robot between rooms and patrolling in map. This status uses the update_ontology() function defined in build_ontology_map.py node to fetch data related to target_room and battery_state of robot ,meanwhile updates the list of time visited rooms. The robot movement is controlled through base_movement() function defined in set_object_state.py node. If robot reached target base_movemet() becomes False and machine state changes to INSPECT_ROOM. During patrolling, in case battery level of the robot is lower than thershold the target_room is cancel and machine state changes to GO_TO_CHARGER.

execute(userdata)

Implements the execution of the tasks while this state gets active.

class scripts.finite_state_machine.GO_TO_CHARGER

This state is triggered as a result of the message received from GO_IN_ROOM state when battery is low. In this state target_position becomes room #E and update_ontology() function updated and room #E add as a first room in URGENT CLASS. if room #E is reached state machine changes to WAIT_FOR_CHARGING.

execute(userdata)

Implements the execution of the tasks while this state gets active.

class scripts.finite_state_machine.INSPECT_ROOM

This state starts working when robot reached center of target_room. The arm_movement() becomes True to detect the room. After complete inspection, state machine changes again to GO_IN_ROOM. N.B: The base_movment() is False in case robot base start to move, this cause due to high frequency of the base control.

execute(userdata)

Implements the execution of the tasks while this state gets active.

class scripts.finite_state_machine.SETUP_MAP

SETUP_MAP is initial state of hierarchical state machines, this state starts to move robot manipulator use my_moveit.cpp node. In this node all the configuration needed to find the boxes with IDs in different position are defined. Also in this state use arm_movement() function defined in set_object_state.py node. This last one is useful to change the state of the manipulate (True/False). When 7 ID’s are detected the state machine goes in next state and manipulator change state to False and machine state changes to GO_IN_ROOM.

execute(userdata)

Implements the execution of the tasks while this state gets active.

class scripts.finite_state_machine.WAIT_FOR_CHARGING

When room #E is reached by robot, robot wait 5 sec to charge battery and update onology map and state machine changes to GO_IN_ROOM and start again patrolling. In this state uses battery_lvl() function defined in set_object_state.py node to set battery value of the robot.

execute(userdata)

Implements the execution of the tasks while this state gets active.

scripts.finite_state_machine.go_to(pose)

Use Simple Action Client to send goal in move_base node, and gets a target_pose as an argument and sends it as MoveBaseGoal.msg to the action server. :param pose: :type pose: Point

Returns:result(MoveBaseResult.msg)

scripts.finite_state_machine.in_target(target_pose)

This function it’s useful just to control if robot reach target postion. Use GetPose service to get current postion of robot and control in function of proximity threshold. In case robot reach target position state of this function change to True. :param target_pose: :type target_pose: Point

Returns:target_reached(Bool)

scripts.finite_state_machine.main()

The main function to run finite state machine, add state to each contatiner, create and start the introspection server for visualization smach_ros, initialises the node and takes an instance of Build_Ontology_map class in the time instance now, defines the subscriner to the /image_id topic.

scripts.finite_state_machine.marker_cb(data)

Marker CallBack function subscribe to image_id topic. Each time an image ID is obtained through robot_camera/image_box it checks if the ID detected already exists or is significant and saves it in the following form:

1. Save value of room_id detected (the value must be between 11 and 17 otherwise it is an error)
2. Save name of room detected (i.e. #E, #R1, #R2, #R3, #R4, #C1, #C2)
3. Save numer of room detected (there are max 7 IDs)

4. Save room_data in room_info list to find out the connections between room, the door assigned to each room,

   and last visited time for each room

Data about each room updates in the helper scripts build_ontological_map.py :param data: :type data: int32

scripts.finite_state_machine.room_data(room_id)

room_data function call service RoomInformation to get information of each room, use room_info. :param room_id: :type room_id: int

Returns:resp(RoomInformationResponse)

scripts.finite_state_machine.room_position(room)

Use previous function room_data to find out center of each room use room_pose x and y position.

Parameters:room (string) – Returns:room_pose(Point)