As you have seen throughout this book, the cmd_vel
topic (the geometry_msgs/Twist
message) is the common control method for ROS robots, whether driving on the ground or flying in the air. For TurtleBot, mobile_base_commands/velocity
and cmd_vel_mux/input/navi
are used to move around the base. For Crazyflie, the crazyflie/cmd_vel
topic is published to control the flight of the quadrotor.
Within the crazyflie_autonomous
package, the crazyflie_controller
node (control_crazyflie.py
) determines the Crazyflie's control state and publishes the crazyflie/cmd_vel
topic. To launch the crazyflie_controller
node, the control_crazyflie.launch
file is used. This launch file also launches the crazyflie_window
node that observes the Crazyflie and takes action when it flies near the edge of the Kinect image frame. The function of this node is described in the subsequent section, using an observer mode.
The crazyflie_controller
node has five states of flight control: idle, takeoff, hover, flight, and land. The private variable _cf_state
is used to indicate the current control state. Regardless of the control state, the cmd_vel
topic is published at a rate of 50 hertz (20 milliseconds). This rate is obtained as the frequency parameter from the Parameter Server and can be changed by either adding this parameter and a new value to the crazyflie_controller
node in the control_crazyflie.launch
file, or using the rosparam set
command in the terminal window command line. The main launch file for this mission is hover_kinectv2.launch
.
With respect to the state of control, the fields for the cmd_vel
topic (the geometry_msgs/Twist
message) are assigned linear velocity values of x
, y
, and z
, and the angular velocity values are left at zero. Recall from Chapter 7, Making a Robot Fly, that the data fields for the Crazyflie cmd_vel
topic are as follows:
The contents of these data fields and the operation of the control states are described in detail throughout the following sections.
The control states of takeoff and land are activated using ROS service calls. Within the crazyflie_controller
node, two ROS services are created with callback functions to be invoked by a client when a request for the service is sent. The services for /crazyflie/land
and /crazyflie/takeoff
are created by the following statements in control_crazyflie.py
:
s1 = rospy.Service("/crazyflie/land", Empty, self._Land) s2 = rospy.Service("/crazyflie/takeoff", Empty, self._Takeoff)
Note that the /crazyflie
namespace has been appended to these services to identify that they are specific for the quadrotor. Land
and Takeoff
are private callback functions that handle the service requests.
These services are of the type Empty
, one of the service types provided by the ROS std_srvs
package. The std_srvs
package contains common service patterns for signals to a ROS node. The Empty
service definition contains no actual data, but is used only to cause the execution of a function.
For the land service, the following function is executed:
def _Land(self, req): rospy.loginfo("Landing requested!") self._cf_state = 'land' return ()
When the /crazyflie/land
service is requested, the loginfo
function writes a log message to stdout
(the terminal window) and to the /rosout
topic. The message also appears in the ~/.ros/log
file for the crazyflie_controller
node. The next statement changes the Crazyflie control state to land
. An Empty
service response message is returned to the client node.
The takeoff
service is handled by a function similar to _Land
. It also writes a log message and changes the Crazyflie control state to takeoff
. An Empty
service response is sent back to the client node.
The services of takeoff and land can be activated using the Xbox 360 joystick controller. The hover_kinectv2.launch
file launches the node for joystick_controller
, which contains requests for Crazyflie takeoff, land, and emergency. These service requests are activated by pressing the blue (takeoff), green (land), or red (emergency) buttons on the Xbox 360 controller. The emergency service request is handled by the crazyflie_server
node (crazyflie_server.cpp
in the crazyflie/crazyflie_driver
package). The code for the joystick_controller
node is found in controller.py
in the crazyflie/crazyflie_demo
package.
The flight controls for takeoff and land are part of the state-based logic of the iteration function of control_crazyflie.py
. When _cf_state
is idle, the linear velocity values of x
, y
, and z
(pitch, roll, and thrust, respectively) are set to 0.0
. The thrust variable is also set to 0.0
. The location of the Crazyflie received as a transform is saved in the takeoff_position
variable. This takeoff_position
variable is used during the takeoff
control state.
When the _cf_state
control state is takeoff, the cmd_vel
linear velocity values of x
and y
are set to 0.0
. The vertical value y
of the takeoff_position
variable (takeoff_position[1]
) is used to compute an upper takeoff height of 25 pixels in y
, above its takeoff y
value. When Crazyflie's position in the Kinect's image frame has achieved that height, the _cf_state
control state will transition to flight. If the value of the thrust
variable exceeds 50,000, this condition will also transition the _cf_state
from takeoff
to flight
.
During takeoff, the value of the cmd_vel
linear z
velocity (thrust) is incremented by 10,000 multiplied by a delta time dt
and a fudge factor ff
. The delta time is computed as the time between the last iteration cycle and the present iteration cycle, which is typically 0.02 seconds (based on 50 hertz). The fudge factor is an easy way to vary the amount of thrust increase applied. When the value of the thrust reaches 36,000, the increments of the additional thrust decrease by approximately one-third to slow the ascent of the Crazyflie.
When the upper takeoff height is achieved, or the thrust is greater than 50,000, the previous error and time values for the PID controllers are reset to zero. The initial integral value for the z
PID controller is set to the following:
(current thrust value - 1500) / (ki for the z PID controller)
Success messages are sent to the log file and the /rosout
topic to indicate takeoff is achieved. Info messages are also sent to log the data being published in the cmd_vel
messages.
The control states of hover and flight utilize the PID class constructor, attributes, and methods from pid.py
, and data from crazyflie2.yaml
. There are three PID objects created to provide proportional, integral, and derivative control for Crazyflie's linear x
, y
, and z
(pitch, roll, and thrust) values. The crazyflie_controller
node instantiates a separate flight PID controller for X
, Y
, and Z
, as shown in the following statements:
from pid import PID # for PID class, attributes and methods # object instances of type PID with initial attributes assigned self.m_pidX = PID(rospy.get_param("~PIDs/X/kp"), rospy.get_param("~PIDs/X/kd"), rospy.get_param("~PIDs/X/ki"), rospy.get_param("~PIDs/X/minOutput"), rospy.get_param("~PIDs/X/maxOutput"), rospy.get_param("~PIDs/X/integratorMin"), rospy.get_param("~PIDs/X/integratorMax")) self.m_pidY = PID(rospy.get_param("~PIDs/Y/kp"), rospy.get_param("~PIDs/Y/kd"), rospy.get_param("~PIDs/Y/ki"), rospy.get_param("~PIDs/Y/minOutput"), rospy.get_param("~PIDs/Y/maxOutput"), rospy.get_param("~PIDs/Y/integratorMin"), rospy.get_param("~PIDs/Y/integratorMax")) self.m_pidZ = PID(rospy.get_param("~PIDs/Z/kp"), rospy.get_param("~PIDs/Z/kd"), rospy.get_param("~PIDs/Z/ki"), rospy.get_param("~PIDs/Z/minOutput"), rospy.get_param("~PIDs/Z/maxOutput"), rospy.get_param("~PIDs/Z/integratorMin"), rospy.get_param("~PIDs/Z/integratorMax")) self.m_pidYaw = PID(rospy.get_param("~PIDs/Yaw/kp"), rospy.get_param("~PIDs/Yaw/kd"), rospy.get_param("~PIDs/Yaw/ki"), rospy.get_param("~PIDs/Yaw/minOutput"), rospy.get_param("~PIDs/Yaw/maxOutput"), rospy.get_param("~PIDs/Yaw/integratorMin"), rospy.get_param("~PIDs/Yaw/integratorMax"))
A PID controller is also created for yaw control but is not used at this time. The values of the parameters kp
, kd
, ki
, minOutput
, maxOutput
, integratorMin
, and integratorMax
are loaded from the crazyflie2.yaml
file as part of the control_crazyflie.launch
process. This arrangement of loading the parameters from the YAML file has made it quick and easy to change parameters while testing flight control.
The PID
class has several methods to perform operations for the PID controller object instance. A method to reset the controller is provided by the reset method. This method sets the m_integral
and m_previousError
values to zero and the m_previousTime
to the current time. The setIntegral
method sets the m_integral
value to a value passed to the function. The third method update performs the PID calculations between the current location and the target location, as shown in the following statements:
def update (self, value, targetValue): time = float(rospy.Time.to_sec(rospy.Time.now())) dt = time - self.m_previousTime error = targetValue - value self.m_integral += error * dt self.m_integral = max(min(self.m_integral, self.m_integratorMax), self.m_integratorMin) p = self.m_kp * error d = 0 if dt > 0: d = self.m_kd * (error - self.m_previousError)/dt i = self.m_ki * self.m_integral output = p + d + i self.m_previousError = error self.m_previousTime = time return max(min(output, self.m_maxOutput), self.m_minOutput)
Note that rospy.loginfo
statements have been removed to enhance clarity.
In the update
method, the current time in seconds is determined by a call to the rospy
routines, Time.now
and Time_to_sec
. The variable dt
is set to the number of seconds elapsed between the last call to the controller and the current time. The difference between value
and targetValue
is stored as the variable error
. This error value is multiplied by m_kp
to obtain the proportional variable p
. The difference in this error value and the last error value is calculated and divided by the delta time dt
. This value is multiplied by m_kd
to find the derivative term d
. The last term, the integral i
, is calculated as the value of m_ki
times m_integral
. The three terms p
, i
, and d
are added to compute the output
variable. This variable is compared to the m_minOutput
and m_maxOutput
values to determine whether it falls within this range. If it does, then the value of output
is returned. Otherwise, if the output
value is larger than m_maxOutput
, m_maxOutput
is returned. If it is less than m_minOutput
, m_minOutput
is returned.
Throughout the testing phase for this mission, Crazyflie exhibited some erratic behavior. Due to the modular nature of ROS, we decided to implement an observer node that would keep track of the location of Crazyflie. The crazyflie_window
node (in watcher.py
) listens to the tf transforms, publishing the location of Crazyflie. In a loop that runs at 10 times a second, the following statements are executed:
if listener.frameExists(camera_frame) and listener.frameExists(crazyflie_frame): t = listener.getLatestCommonTime(camera_frame, crazyflie_frame) trans, rotate = listener.lookupTransform(camera_frame, crazyflie_frame, t)
This code checks the transforms that are buffered by the listener for the existence of a transform between crazyflie/baselink
and kinect2_ir_optical_frame
. When this specific transform is found, the data fields for translational and rotational data are extracted into the trans
and rotate
variables. The trans
variable contains the location in the x, y, and z of the Crazyflie. This location is compared to the edge of the Kinect image:
if (trans[0] < 100) or (trans[0] > (camera_width - 100)) or (trans[1] < 20) or (trans[1] > (camera_height - 20)): # Crazyflie is going outside the frame rospy.loginfo("Crazyflie outside of window %f %f %f", trans[0], trans[1], trans[2]) rospy.loginfo("Landing requested") # wait until land service is available, then create handle for it rospy.wait_for_service('/crazyflie/land') try: _land = rospy.ServiceProxy('/crazyflie/land', Empty) _land() except rospy.ServiceException, e: rospy.loginfo("Service call failed: %s", e)
If the position of Crazyflie is within 100 pixels of the left or right edge of the image frame, or within 20 pixels of the upper or lower edge, a service request is made for Crazyflie to land. A private local proxy _land
is used to make the service call with an Empty
service request. The land service request is handled by the crazyflie_controller
node as described in the previous section, Using ROS services to control takeoff and land.
Messages are sent to the log file and the /rosout
topic to identify the location of the Crazyflie that caused the crazyflie_window
node to send the service request. These messages are important when determining the events of Crazyflie's flight. The Kinect's depth data trans[2]
is too erratic to use for this monitoring instance.
The next sections describe how the Crazyflie operates when the _cf_state
variable is set to flight
. The Crazyflie will either hover in place or fly to a target depending on whether any target_pose
messages have been received.