Skip to content

3.5 Planner

Jump to bottom
frompotenza edited this page Jun 17, 2024 · 20 revisions

Planner

Table of Contents

Overview

The Planner is responsible for high-level decision making/mission planning for the AUV. It uses simplified interfaces with the Vision and State Estimation packages to decide what the AUV should do next, and an interface with the Controls package to execute changes in position/orientation accordingly. Missions are built as state machines, i.e. a series of states, where each state represents a small task (for example searching for a specific object on the floor of the pool, going through a gate, etc.).

Usage

Write missions in the src/ folder as standalone Python files or as substate classes (in src/substates) using the utility classes mentioned below. Then, create a launch file for your mission.

Utility Classes

Control utility class (Controller)

NOTE: for some of these functions, an optional callback argument can be passed. If a callback function is passed, the function will return instantly, and the callback will be called later when the action has been completed. Otherwise, if no callback function is passed, the function will not return until the action is complete.

NOTE: For all (or most) of these functions, you can "ignore" certain axes to perform actions on one or a few axes only. To do this, pass None as the value for the axis you want to ignore. For example, Controller.move((None, None, -2)) will move the AUV to z=-2, but will not change it's x/y position. The same applies for Controller.moveDelta((None, None, -2)), this call is equivalent to Controller.moveDelta((0, 0, -2)), i.e. the AUV will descend from it's current position on the z axis by 2 meters.

The Controller class offers the following functions to control the AUV:

  • preemptCurrentAction(): Cancels the action currently being completed.
  • rotate(ang, callback=None): Set's the AUV's orientation to the given quaternion ang, which must be passed as an array of the form [w,x,y,z].
  • rotateEuler(ang, callback=None): Sets the AUV's orientation to the given euler angles ang, which must be passed as an array of the form [x,y,z].
  • rotateDelta(delta, callback=None): Rotates the AUV by the given quaternion, i.e. it adds to the AUV's orientation. delta must be passed as an array of the form [w,x,y,z].
  • rotateDeltaEuler(delta, callback=None): Displaces the AUV's by the given euler angles, i.e. it adds to the AUV's orientation. delta must be passed as an array of the form [x,y,z].
  • move(pos, callback=None, face_destination=False): Moves the AUV to the given world position pos, which must be an array of the form [x,y,z]. If the boolean face_destination is passed as True, the AUV will turn to face it's destination before moving towards it.
  • moveDelta(delta, callback=None, face_destination=False): Displaces the AUV's position by the given offset delta, which must be an array of the form [x,y,z]. If the boolean face_destination is passed as True, the AUV will turn to face it's destination before moving towards it.
  • moveDeltaLocal(delta, callback=None, face_destination=False): Displaces the AUV's position in local space by the given offset delta, which must be an array of the form [x,y,z]. "Local space" means that the offset is defined with respect to the AUV, not the world. This means that the x value of pos defines how much the AUV should move "forward", the y value defines "left" movement, and z defines "up" movement, where "forward", "left" and "up" are defined with respect to the AUV's orientation.
  • state(pos, ang, callback=None): Sets the AUV's orientation and position to the given position pos and quaternion ang. These must be passed as arrays of the form [x,y,z] and [w,x,y,z], respectively.
  • stateDelta(pos, ang, callback=None): Displaces the AUV's orientation and position by the given position pos and quaternion ang. These must be passed as arrays of the form [x,y,z] and [w,x,y,z], respectively.
  • stateEuler(pos, ang, callback=None): Sets the AUV's orientation and position to the given position pos and euler angles ang. These must be passed as arrays of the form [x,y,z] and [x,y,z], respectively.
  • stateDeltaEuler(pos, ang, callback=None): Displaces the AUV's orientation and position by the given position pos and euler angles ang. These must be passed as arrays of the form [x,y,z] and [x,y,z], respectively.
  • torque(vel): Sets the torque of the AUV (in Newtons) to vel, which must be an array of the form [x,y,z].
  • forceLocal(vel): Set's the positional effort/force of the AUV (in Newtons) to vel, which must be an array of the form [x,y]. The z axis is not available for forces since the buoyancy of the AUV means that it would not behave as expected, or would not be useful if the behaviour was expected.
  • kill(): Stops all of the AUV's thrusters. This is used at the end of missions, the AUV will float back up to the surface.
  • stop_in_place(callback=None): "Freezes" the AUV in it's current position and orientation. It will not float back up to the surface.
  • fix_rotation(callback=None): "Freezes" the AUV's orientation only.

Vision utility class (ObjectMapper)

NOTE: "Objects" in the context of this utility class are defined as arrays of the form [label,x,y,z,theta_z,extra_field].
The ObjectMapper class offers the following functions to analyze the AUV's environment:

  • getClass(self,cls=None): returns an array containing all objects of the given label/class cls that the AUV has mapped. If cls is None, this function will return all known objects.
  • getClosestObject(self,pos,cls=None): returns the closest object of the given label/class cls that the AUV has mapped. If cls is None, this function will return the closest object regardless of class.
  • updateObject(self, obj): Given an object, the function will return the same object with the most up-to-date data from the Vision package. This is used since the AUV's estimation of objects' positions, orientations, and "extra_field"s improves in confidence as more observations of the object are made.

State Estimation utility class (State)

The State class does not offer any functions, but rather keeps it's class variables up-to-date so they can be read from directly. Those variables are:

  • x: The AUV's x position.
  • y: The AUV's y position.
  • z: The AUV's z position.
  • theta_x: The x axis field of the AUV's euler angle rotation.
  • theta_y: The x axis field of the AUV's euler angle rotation.
  • theta_z: The x axis field of the AUV's euler angle rotation.
  • pose: A Pose object which has the position and orientation (as a quaternion) of the AUV.

missions.py

missions.py

Launch Files

The launch files in the planner are each responsible for running a specific mission. There is no point in listing them since they all do the same thing: run the planner for a specific mission. The Vision, Control, and State Estimation packages must be running at a minimum, but to actually do anything, the Sim and other packages must also be running. To run these, use the launch files defined in the Bringup package. Then, run a mission launch file.

Dependencies

  • actionlib
  • rospy
  • vision
  • smach
  • tf
  • auv_msgs
  • geometry_msgs
  • std_msgs

Sidebar

Clone this wiki locally