#include <Quadrotor_tk_Handler.h>
Public Member Functions | |
unsigned int | getObjectType () const |
void | handleSimulation () |
Quadrotor_tk_Handler () | |
void | synchronize () |
~Quadrotor_tk_Handler () | |
Protected Member Functions | |
void | _initialize () |
void | tkCommandsCallback (const telekyb_msgs::TKCommands::ConstPtr &msg) |
void | tkMotorCommandsCallback (const telekyb_msgs::TKMotorCommands::ConstPtr &msg) |
Protected Attributes | |
simFloat | _att_cutoff |
simFloat | _att_damping |
CustomDataHeaders::QuadrotorCtrlMode | _ctrlMode |
int | _handleOfCoM |
int | _handleOfJoint [4] |
float | _integralTermPitch |
float | _integralTermRoll |
simFloat | _jointPosition [4][3] |
simFloat | _kp_yaw |
ros::Time | _lastPrintedMsg |
ros::Time | _lastReceivedCmdTime |
ros::Time | _previousTime |
ros::Publisher | _pub |
ros::Publisher | _pubIMU |
simFloat | _quadrotorMass |
ros::Subscriber | _sub |
telekyb_msgs::TKCommands | _tkCommands |
std::vector< double > | _tkMotorCommands |
float | _torqueToForceRatio |
Handler of a Quadrotor object for use with Telekyb2. It automatically subscribes to telekyb_msgs::TKMotorCommands messages for receiveng thrust commands to apply to the four propellers. It also publishes the status of the quadrotor using telekyb_msgs::TKState messages.
Definition at line 30 of file Quadrotor_tk_Handler.h.
Definition at line 16 of file Quadrotor_tk_Handler.cpp.
Definition at line 34 of file Quadrotor_tk_Handler.cpp.
void Quadrotor_tk_Handler::_initialize | ( | ) | [protected, virtual] |
Implements GenericObjectHandler.
Definition at line 397 of file Quadrotor_tk_Handler.cpp.
unsigned int Quadrotor_tk_Handler::getObjectType | ( | ) | const [virtual] |
Implements GenericObjectHandler.
Definition at line 37 of file Quadrotor_tk_Handler.cpp.
void Quadrotor_tk_Handler::handleSimulation | ( | ) | [virtual] |
Implements GenericObjectHandler.
Definition at line 135 of file Quadrotor_tk_Handler.cpp.
void Quadrotor_tk_Handler::synchronize | ( | ) | [virtual] |
Reimplemented from GenericObjectHandler.
Definition at line 69 of file Quadrotor_tk_Handler.cpp.
void Quadrotor_tk_Handler::tkCommandsCallback | ( | const telekyb_msgs::TKCommands::ConstPtr & | msg | ) | [protected] |
Callback for thrust/roll/pitch/yaw command (TKCommands) message reception.
msg | Received thrust/roll/pitch/yaw command message. |
Definition at line 542 of file Quadrotor_tk_Handler.cpp.
void Quadrotor_tk_Handler::tkMotorCommandsCallback | ( | const telekyb_msgs::TKMotorCommands::ConstPtr & | msg | ) | [protected] |
Callback for force command (TKMotorCommands) message reception.
msg | Received force command message. |
Definition at line 532 of file Quadrotor_tk_Handler.cpp.
simFloat Quadrotor_tk_Handler::_att_cutoff [protected] |
Internal roll/pitch control cutoff frequency.
Definition at line 117 of file Quadrotor_tk_Handler.h.
simFloat Quadrotor_tk_Handler::_att_damping [protected] |
Internal roll/pitch control damping factor.
Definition at line 122 of file Quadrotor_tk_Handler.h.
Quadrotor control mode. See QuadrotorCtrlMode.
Definition at line 132 of file Quadrotor_tk_Handler.h.
int Quadrotor_tk_Handler::_handleOfCoM [protected] |
Handle of the robot CoM.
Definition at line 67 of file Quadrotor_tk_Handler.h.
int Quadrotor_tk_Handler::_handleOfJoint[4] [protected] |
Handles of the four propellers.
Definition at line 62 of file Quadrotor_tk_Handler.h.
float Quadrotor_tk_Handler::_integralTermPitch [protected] |
Definition at line 52 of file Quadrotor_tk_Handler.h.
float Quadrotor_tk_Handler::_integralTermRoll [protected] |
The following variables are needed for the Integral part of the PID (roll and pitch respectively)
Definition at line 51 of file Quadrotor_tk_Handler.h.
simFloat Quadrotor_tk_Handler::_jointPosition[4][3] [protected] |
Position of the four propellers w.r.t. the robot CoM.
Definition at line 77 of file Quadrotor_tk_Handler.h.
simFloat Quadrotor_tk_Handler::_kp_yaw [protected] |
Internal yaw rate control proportional gain.
Definition at line 127 of file Quadrotor_tk_Handler.h.
ros::Time Quadrotor_tk_Handler::_lastPrintedMsg [protected] |
Timer for printing messages.
Definition at line 102 of file Quadrotor_tk_Handler.h.
ros::Time Quadrotor_tk_Handler::_lastReceivedCmdTime [protected] |
Timer for received commands.
Definition at line 97 of file Quadrotor_tk_Handler.h.
ros::Time Quadrotor_tk_Handler::_previousTime [protected] |
This variable is needed to store the previousTime to perform an Integral of the angles to use an Integral part into the controller.
Definition at line 47 of file Quadrotor_tk_Handler.h.
ros::Publisher Quadrotor_tk_Handler::_pub [protected] |
Publisher for the robot status.
Definition at line 82 of file Quadrotor_tk_Handler.h.
ros::Publisher Quadrotor_tk_Handler::_pubIMU [protected] |
Publisher for the robot status.
Definition at line 87 of file Quadrotor_tk_Handler.h.
simFloat Quadrotor_tk_Handler::_quadrotorMass [protected] |
Handle of the robot CoM.
Definition at line 72 of file Quadrotor_tk_Handler.h.
ros::Subscriber Quadrotor_tk_Handler::_sub [protected] |
Subscriber for force commands.
Definition at line 92 of file Quadrotor_tk_Handler.h.
telekyb_msgs::TKCommands Quadrotor_tk_Handler::_tkCommands [protected] |
Last received TKCommands message.
Definition at line 112 of file Quadrotor_tk_Handler.h.
std::vector<double> Quadrotor_tk_Handler::_tkMotorCommands [protected] |
Last received TKMotorCommands commands.
Definition at line 107 of file Quadrotor_tk_Handler.h.
float Quadrotor_tk_Handler::_torqueToForceRatio [protected] |
Ratio between force ( ) and torque ( ) produced by the propeller, i.e.
Definition at line 57 of file Quadrotor_tk_Handler.h.