This section will describe the implementation of several nested control systems as ROS nodes. This includes angular rate, attitude, and position controllers. The theory behind the design of these controllers is described in detail in the Controller Design section and specific implementation details are covered in the Simulation Environment section.
Each controller acts as a node, subscribing to and publishing topics. This code is contained in the quad_control folder and can be visualized using rqt_graph. The control code further broken down between the node and library folders. The code to set up and run each node is separate from the control calculation code. The library file, quad_controller.cpp, contains a class for each controller and a utility class. Each controller class contains an initialization function and a control calculation function. This includes the use of rosparam YAML files for loading and importing control gains from the ROS Parameter Server.
Joystick Interface
An Xbox 360 controller was used to mimic the manual control inputs of a physical RC system. These control commands were used to update the set points of the various controllers. First, the joy package was used to read the button and stick values on the controller and publish a Joy message containing these values. This message is received by the quad_joystick_interface node modified from the RotorS package. The joy.cpp file creates this node. The node converts the Xbox controller values to desired roll and pitch angles, generalized thrust value, and a yaw rate which is then published in a CommandRollPitchYawrateThrust message by default. If GPS mode is selected, the node converts the Xbox controller values to global position increments and publishes a CommandTrajectory message. The node also supports sending commands such as takeoff, landing, and enabling waypoint mode. This interface node can be extended to support additional features as the software develops.
The except below shows the syntax for starting these nodes in a launch file. Note the two additional parameters for the joy node allow the user to set a dead zone and specify the specific device location.
<group ns="quad">
<node name="joy_node" pkg="joy" type="joy_node" >
<param name="dev" type="string" value="/dev/input/js1" />
<param name="deadzone" value="0.12" />
</node>
<node name="quad_joystick_interface" pkg="quad_joystick_interface" type="quad_joystick_interface" output="screen"/>
</group>
Attitude and Angular Rate Controllers
The low level rate and attitude controllers are implemented in the attitude_controller_node. This node subscribes to the IMU message for the current rotation and translation measurements and a CommandRollPitchYawrateThrust message for the desired attitude and thrust. This node uses the quad_controller.cpp library to first implement the attitude controller. This includes independent PID controllers for roll, pitch, yaw, and thrust that are computed for each new IMU measurement. These controllers compute desired angular rates which are then passed to the angular rate controller. The rate controller computes the desired angular moments which are then mixed with the desired thrust value and mapped to each motor. Finally, the ComandMotorSpeed message is published which contains the desired speed of each motor in rad/s.
The quad_joystick_empty_world.launch file creates a simulation with the user providing desired attitude commands via the joystick to the attitude controllers on the quadrotor. These nodes are detailed in the launch file excerpt below. This excerpt also shows the ability to load the model and control parameters to the parameter server that are then input into the attitude control node.
<group ns="quad">
<node name="joy_node" pkg="joy" type="joy_node" />
<node name="quad_joystick_interface" pkg="quad_joystick_interface" type="quad_joystick_interface" />
<node name="attitude_controller_node" pkg="quad_control" type="attitude_controller_node" output="screen">
<rosparam command="load" file="$(find quad_gazebo)/resource/kit_c.yaml" />
<rosparam command="load" file="$(find quad_control)/resource/attitude_controller.yaml" />
</node>
</group>
These nodes and topics can be visualized with the following code which launches the joystick and attitude control nodes on the quadrotor model in an empty world and visualizes the nodes and topics.
roslaunch quad_gazebo quad_joystick_empty_world.launch rosrun rqt_graph rqt_graph
Position Controllers
The position controllers are implemented in the position_controller_node. This node receives global position and orientation by subscribing to an Odometry message which acts like a GPS sensor. This node receives a desired global position and heading from a CommandTrajectory message in the form of a waypoint. The node implements independent PID controllers for each transnational axis and heading which is computed each time a new odometry message is received. The position controller rotates the current position measurements and desired waypoint from Global to Body frame and computes the desired thrust, attitude, and heading and publishes a CommandRollPitchYawrateThrust message. Note that the position controller only publishes control commands if the user has entered GPS mode by pushing the correlated button on the Xbox controller.
Sending Waypoints
The waypoint_node is used to compute and send the desired waypoint to the position_controller_node. This node receives a CommandTrajectory message which contains the desired global position offsets. The node also subscribes to the Odometry message emulating a GPS sensor. For each odometry measurement received, the waypoint_node combines the quadrotor’s current position and heading with the desired offsets to compute a new global waypoint. The CommandTrajectory message was modified to contain values associated with the “launch”, “land” and “waypoint mission” modes. The “take off” mode sets the desired waypoint altitude to 1 meters while the “land” mode sets the desired waypoint altitude to 0 meters without modifying the x, y, or yaw values. The “waypoint mission” mode reads in a waypoint file that contains a series of waypoints and the time for them to be sent. If enabled, the waypoint node will send each waypoint at the defined time. This allows users to develop and implement autonomous waypoint missions to navigate the quadrotor around the Gazebo world.
The quad_joystick_gps_empty_world.launch file creates a simulation with the user providing input via the controller. The user is then able to switch between sending desired positions or desired attitudes to the quadrotor and the appropriate onboard controllers. These nodes are detailed in the launch file excerpt below.
<group ns="quad">
<node name="joy_node" pkg="joy" type="joy_node" />
<node name="quad_joystick_interface" pkg="quad_joystick_interface" type="quad_joystick_interface" output="screen"/>
<node name="waypoint_node" pkg="quad_control" type="waypoint_publisher_node" output="screen"/>
<node name="position_controller_node" pkg="quad_control" type="position_controller_node" output="screen">
<rosparam command="load" file="$(find quad_gazebo)/resource/kit_c.yaml" />
<rosparam command="load" file="$(find quad_control)/resource/position_controller.yaml" />
</node>
<node name="attitude_controller_node" pkg="quad_control" type="attitude_controller_node" output="screen">
<rosparam command="load" file="$(find quad_gazebo)/resource/kit_c.yaml" />
<rosparam command="load" file="$(find quad_control)/resource/attitude_controller.yaml" />
</node>
</group>
These nodes and topics can be visualized with the following code which launches the joystick, waypoint, position control, and attitude control nodes on the quadrotor model in an empty world.
roslaunch quad_gazebo quad_joystick_gps_empty_world.launch rosrun rqt_graph rqt_graph
Enabling “GPS” mode and then “launch” mode results in the quadrotor hovering at 1 meter as shown in the Gazebo screenshot below.
Receiving Velocity Commands
The position controller must also be able to receive desired velocity commands in order to be compatible with the path planning algorithms. The path planning algorithms output desired velocities in the form of Twist messages. In order to support this mode, the CommandTrajectory message in the Joystick Interface node was modified to contain a value associated with a “path planning” mode. This mode was mapped to a button on the Xbox controller. When triggered, the waypoint_node received desired velocity commands on the “cmd_vel” topic as well as the quadrotor’s global position from the odometry message and output a desired waypoint. For 2D navigation, only desired x, y, and yaw velocities are published. Only in 3D navigation are desired z velocities published. The rate and scale of the velocity commands can be tuned in the configuration files of the specific path planning algorithm that is used.
A video summarizing this section as well as displaying ROS and Gazebo footage is available below.
