Finite State Machine Node

Subscribes to:

/image_id

Uses Service:

/state/set_battery_level

/state/get_pose

/state/set_base_movement_state

/room_info

/move_arm

Uses Action:

/move_base

Uses helper script:

/utilities/ontological_robot_control/topological_map.py

Defines the states and transitions for the finite state machine of this rospackage. In the initial state robot builds the semantic map of environment using image id of the markers detected by robot camera. This node uses topological_map.py helper script to update the ontology while the process is running, and retreives the target room based on last visit times and robot battey state. In the next state it moves to the target room and if battery level gets lower than threshold, it goes to charger, and charges the battery.

class scripts.finite_state_machine.BuildSemanticMap

Defines the initial state when robot builds the semantic map.It enables the robot arm to start moving on the desired trajectories as it is defined in my_moveit.cpp file using set_arm_movement_state(arm_movement_state) function, and then checks if the number of detected room ids are enough, then exits from state by returning map_ready

execute(userdata)

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

class scripts.finite_state_machine.ExploreRoom

Defines the state when robot has reached the target room and then explores it. It enables robot arm movement like the initial state using set_arm_movement_state(arm_movement_state) functinon, then returns room_explored

execute(userdata)

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

class scripts.finite_state_machine.MoveToCharger

Defines the state when battery level is low and moves to charger, first enables battery consumption using set_base_movement_state(movement_state) function and then moves to charger using move_to_pose(pose) function. Additionally, it updates the ontology while it moves to charger using update_ontology(now) function, until it reaches the charger.

execute(userdata)

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

class scripts.finite_state_machine.MoveToTargetRoom

Defines the state when robot moves to target room found by the ontology, first enables battery consumption using set_base_movement_state(base_movement_state) function and then moves to target room using move_to_pose(pose) function. Additionally, it updates the ontology while it moves to target room using update_ontology(now) function, until it reaches the target room, then exits the state by returning target_reached. If the battery level gets lower than threshold, it returns battery_low and target room will be canceled.

execute(userdata)

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

class scripts.finite_state_machine.Recharging

Defines the state when robot has reached the charger and chargers battery after some time using set_battery_level(battery_level) function and then returns robot_charged transition.

execute(userdata)

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

scripts.finite_state_machine.check_target_reached(target_pose)

Checks if the robot has reached to a specific point by computing the eucledian distance between robot current pose and target pose

Parameters

target_pose (Point) –

Returns

target_reached(Bool)

scripts.finite_state_machine.get_room_info(room_id)

Server client for marker_server, gets information for each room using room_info service

Parameters

room_id (int) –

Returns

resp(RoomInformationResponse)

scripts.finite_state_machine.get_room_pose(room)

Detects the center postion by using the room information for corresponding room

Parameters

room (string) –

Returns

room_pose(Point)

scripts.finite_state_machine.main()

The main function for finite_state_machine node, initialises the node and takes an instance of TopologicalMap class in the time instance now, defines the subscriner to the /image_id topic , defines the states and transitions of the finite state machine for topological map and finally starts the finite state machine process

scripts.finite_state_machine.marker_id_callback(data)

Callback function for /image_id topic subscriber. Eveytime an image id is detected, checks if image id is valuable and not already available, then saves the corresponding information of each room in the global variables by calling get_room_info(room_id) function, and modifies the ontology using add_room(room), add_door(door), assign_doors_to_room(room, doors) disjoint_individuals() and add_last_visit_time(room, visit_time) functions from topological_map.py helper script.

Parameters

data (int32) –

scripts.finite_state_machine.move_to_pose(pose)

Action client function for move_base node, gets a pose as an argument and sends it as MoveBaseGoal.msg to the action server

Parameters

pose (Point) –

Returns

result(MoveBaseResult.msg)

scripts.finite_state_machine.set_arm_movement_state(arm_movement_state)

Service client function for /move_arm. Updates the current robot arm movement state stored in my_moveit node

Parameters

arm_movement_state (bool) –

scripts.finite_state_machine.set_base_movement_state(base_movement_state)

Service client function for /base_movement_state. Updates the current robot base movement state stored in robot-states node

Parameters

base_movement_state (bool) –

scripts.finite_state_machine.set_battery_level(battery_level)

Service client function for /state/set_battery_level Update the current robot battery level stored in the robot-state node

Parameters

battery_level (int) –