Table of Contents
AI
Main function of AI on the sub is to make decisions of what the sub should do based on given data from topics like sensors, vision, data, etc. It is using hierarchical state machine, which allows it to cover majority of cases it will face.
Overview
Current AI uses SMACH package that is in ROS. SMACH is a task-level architecture for rapidly creating complex robot behavior. At its core, SMACH is a ROS-independent Python library to build hierarchical state machines.
Advantages of SMACH are:
- rapid development, ability to create complex state machines
- ability to quickly change state machines without big code changes
- explicitly define outcomes of every state thus covering most or all possible situations
Current AI
Our current AI was re-written using SMACH. There are several utility files such as:
- gate_util.py - all states that are used by gate AI, they are generic.
- util.py - contains utility functions for vision to filter labels, get N most probably, normalize coordinates from vision, or wrap yaw. Note that vision will be changed in future, some of the functions will no longer be useful.
- basic_states.py - contains all of the states for roulette and dice AI.
- control_wrapper.py - wrapper made to ease communication with control system, making it easy to send basic commands such as dive, yaw, pitch, roll, move forward.
- start_switch.py - every high-level state machine must have start_switch as their first state. It is a state that waits for ros message to be sent over topic /start_switch to be true at least 3 times.
- blind_movement.py - contains move_forward state that moves forward with x speed for y number of seconds.
- SubscribeState.py - a state that was made which accept also topic to which you want to subscribe. It is also modified to pass over any input/output keys. In future this file will also contain SynchronousSubscribeState that subscribes to two topics and moves once it has two
There is a useful tool to see state machine and transitions of it called smach_viewer. To run it, run
rosrun smach_viewer smach_viewer.py
An example of what our AI looks like in smach_viewer is this screenshot below
Things to know when developing AI
- When inheriting from SubscribeState instead of SmachState, you need to use self.exit(“outcome”) instead of return “outcome”
- Every python script that is going to run any state machine must have in its main function these lines of code below.
while rospy.get_time() == 0: continue
- Every time you create control_wrapper instance, you need to set depth value again.
- If your state is using control wrapper to move, right before final outcome make sure to set changed yaw/roll/pitch/forward to 0.
- Control wrapper forward and strafe do not use relative same way as yaw and pitch, use instead strafeLeftError() and forwardError()
- Some current AI files use parameters from roscore server for different configuration of values. They will crash if they do not load parameters. Parameters are located in /ros/robosub/param/ or some may be located in individual utility folders. To load parameters run
rosparam load [param_file_name].yaml
- Every time you restart roscore you need to reload parameters
- Our vision detection requires undistortion to be running.
Example Flow Chart
graph LR;
A(SM_ROOT)-->|execute|B[START_SWITCH];
subgraph RoboSub
B-->C[GATE_TASK];
C-->D[PATH_MARKER_TASK1];
D-->E[DICE_TASK];
E-->F[PATH_MARKER_TASK2];
F-->G[GOTO_PINGER];
G-->H{Where am I?};
H-->|Roulette|I[ROULETTE_TASK];
H-->|Square|J[SQUARE_TASK];
end
I-->EN((end))
J-->EN((end))
click B "start_switch"
click C "gate_task"
click D "path_marker_task"
click E "dice_task"
click F "path_marker_task"
click G "goto_pinger"
click I "roulette_task"
click J "square_task"
style A fill:#f9f,stroke:#333,stroke-width:2px;
style EN fill:#f9f,stroke:#333,stroke-width:2px;