LOS guidance backstepping
Jaehyeong Hwang started writing on 28.11.2020
breaking down the code of los_guidance_backstepping.py
vortex_msgs.msgs
contains all custom ROS message types used in all workspaces
PropulsionCommand : Provides normalized motion command in float64 type and control mode in bool type
nav_msgs
defines the common messages used to interact with the navigation stack.
Odometry : represents an estimate of a position and velocity in free space.
geometry_msgs
provides messages for common geometric primitives such as points, vectors, and poses. These primitives are designed to provide a common data type and facilitate interoperability throughout the system.
Wrench : This represents force in free space, separated into its linear and angular parts.
PoseStamped : A Pose with reference coordinate frame and timestamp.
Pose : A representation of pose in free space, composed of position and orientation.
tf.transformations.euler_from_quaternion(quaternion, axes='sxyz')
Return Euler angles from quaternion for specified axis sequence.
tf.transformations.quaternion_from_euler(ai, aj, ak, axes='sxyz')
Return quaternion from Euler angles and axis sequence.
Dynamic_reconfigure
purposes on providing a standard way to expose a subset of a node's parameters to external reconfiguration
from los_guidance.cfg import AutopilotConfig
Describes parameters such as look-ahead distance, proportional gain, integrarl gain, derivative gain,saturation
actionlib
actionlib stack provides a standardized interface for interfacing with preemptable tasks. For example, moving the base to a target location. actionlib will import class SimpmleActionServer, functions register_goal_callback, register_preempt_callback, start, publish_feedback, set_succeeded, is_preempt_requested, set_preempted, accept_new_goal These are all described in https://github.com/strawlab/ros_common/blob/master/actionlib/src/actionlib/action_server.py
cannot find LosPathFollowingAction, LosPathFollowingGoal, LosPathFollowingFeedback
AutopilotBackstepping
This refers to BacksteppingDesign in the file backstepping_control.py, which defines acceleration proportional values and velocity proportional values that should be used in mass matrix M.class AutopilotBackstepping updates gains for the system inertia matrix M. System inertia matrix M=M_RB+M_A for underwater vehicle follows symmetry considerations
AutopilotPID : defines basic pid controller
from reference_model.discrete_tustin import ReferenceModel
From discrete_tustin.py class ReferenceModel, determines the reference model by using Tustin’s method. Tustin's method was used for transform continuous time system to discrete time system in reference model. I won't go into too much in this theory. But by choosing the damping ratio as 1.0 and natural frequency as 0.1 we can get a critically damped and rapid reference model. By using Matlab, we can get the transfer function.
updateState
is used in callback function, specified in line 213. It will update position and velocities.
setWayPoints
updates next way points.
Distance
calculates distance between current position and the next waypoint.
sphereofAcceptance
suggests condition of the distance that is allowable.
getEpsilonVector
resulted epsilon vector is the coordinates of the AUV in the path-fixd reference frame for a straight line going from the reference point to target position the path-tangential angle alpha is used for rotation matrix. transpose matrix of Rotation matrix is denoted as R_T and is used to calculate epsilon vector.
quat2euler
Describes transformation from quaternion to euler angle. Euler_from_quaternion From tf.transformations, euler angle yaw can be calculated.
lookaheadBasedSteering
lookahead-based steering is less computationally intensive than enclosure-based steering.
In order to get to the intersection point p_los,
- manta has to turn its angle as parallel as the straight line segment. It is expressed as self.chi_p.
- Then, it has to turn its heading directly to the p_los. The angle is expressed as self.chi_r
For lookahead-based steering, the course angle assignment is separated into two parts
which is a path-tangential angle, while,
So the desired heading self.chi_d is calculated from the sum of path-tangential angle and velocity-path relative angle. ~~_feedback, _result : cannot find the source~~
rospy.init_node('los_path_following')
Initializes ROS node by specifying name of the node as los_path_following.
self.sub = rospy.Subscriber('/odometry/filtered',Odometry, self.callback, queue_size=1)
‘/odometry/filtered’ signifies the topic name, we are going to subscribe this topic. Odometry topic is going to be used for this. Queue_size is the size of outgoing message queue used for asynchronous publishing. A good queue size is different depending on variables. Briefly speaking, bigger queue size will only use more memory
self.pub_thrust = rospy.Publisher('/manta/thruster_manager/input', Wrench, queue_size=1)
Node is publishing to the /manta/thruster_manager/input, using the message type Wrench, with queue size 1.
self.pub_desired = rospy.Publisher('/manta/los_desired', Odometry, queue_size=1)
As same as the code in line 164, this node is publishing to the /manta/los_desired , using Odometry message type with queue size 1
line 168 ~ line 171 assigns requisite classes.
unclear * * * Geometric task should be satisfied. It is to force the system output to converge to a desired path parametrized by a continuous scalar variable
IN order to reference model work, we have to put heading angle as continuous. However arctangent function, which was used to obtain heading angle is discontinuous at -pi and pi. So before being input to reference model, \psi_ref is being wrapped. From line 197 to 198 Tustin method which was also used in pd_quaternion controller, is being used to calculate desired heading. Result of reference model \psi-d then will be un-wrapped from line 200 to 210.
will write more..