#include <gazebo_fw_dynamics_plugin.h>
Public Member Functions | |
GazeboFwDynamicsPlugin () | |
Constructor. | |
virtual | ~GazeboFwDynamicsPlugin () |
Destructor. | |
Protected Member Functions | |
void | Load (physics::ModelPtr _model, sdf::ElementPtr _sdf) |
Called when the plugin is first created, and after the world has been loaded. This function should not be blocking. | |
double | NormalizedInputToAngle (const ControlSurface &surface, double input) |
Convert control surface input that is normalized in range [-1, 1] to a physical deflection angle value. | |
void | OnUpdate (const common::UpdateInfo &) |
This gets called by the world update start event. | |
void | UpdateForcesAndMoments () |
Calculates the forces and moments to be applied to the fixed-wing body. | |
Private Member Functions | |
void | ActuatorsCallback (GzActuatorsMsgPtr &actuators_msg) |
Processes the actuator commands. | |
void | CreatePubsAndSubs () |
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. | |
void | RollPitchYawrateThrustCallback (GzRollPitchYawrateThrustMsgPtr &roll_pitch_yawrate_thrust_msg) |
Process the roll_pitch_yawrate_thrust commands. | |
void | WindSpeedCallback (GzWindSpeedMsgPtr &wind_speed_msg) |
Processes the wind speed readings. | |
Private Attributes | |
gazebo::transport::SubscriberPtr | actuators_sub_ |
Subscriber for receiving actuator commands. | |
std::string | actuators_sub_topic_ |
Topic name for actuator commands. | |
FWAerodynamicParameters | aero_params_ |
The aerodynamic properties of the aircraft. | |
double | delta_aileron_left_ |
Left aileron deflection [rad]. | |
double | delta_aileron_right_ |
Right aileron deflection [rad]. | |
double | delta_elevator_ |
Elevator deflection [rad]. | |
double | delta_flap_ |
Flap deflection [rad]. | |
double | delta_rudder_ |
Rudder deflection [rad]. | |
bool | is_input_joystick_ |
Are the input commands coming from a joystick (as opposed to a remote control via HIL interface, for example)? | |
physics::LinkPtr | link_ |
Pointer to the link. | |
physics::ModelPtr | model_ |
Pointer to the model. | |
std::string | namespace_ |
Transport namespace. | |
transport::NodePtr | node_handle_ |
Handle for the Gazebo node. | |
bool | pubs_and_subs_created_ |
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from be called on every OnUpdate(). | |
gazebo::transport::SubscriberPtr | roll_pitch_yawrate_thrust_sub_ |
Subscriber for receiving roll_pitch_yawrate_thrust commands. | |
std::string | roll_pitch_yawrate_thrust_sub_topic_ |
Topic name for roll_pitch_yawrate_thrust commands. | |
double | throttle_ |
Throttle input, in range from 0 to 1. | |
event::ConnectionPtr | updateConnection_ |
Pointer to the update event connection. | |
FWVehicleParameters | vehicle_params_ |
The physical properties of the aircraft. | |
ignition::math::Vector3d | W_wind_speed_W_B_ |
Most current wind speed reading [m/s]. | |
gazebo::transport::SubscriberPtr | wind_speed_sub_ |
Subscriber ror receiving wind speed readings. | |
std::string | wind_speed_sub_topic_ |
Topic name for wind speed readings. | |
physics::WorldPtr | world_ |
Pointer to the world. |
Definition at line 50 of file gazebo_fw_dynamics_plugin.h.
Constructor.
Definition at line 24 of file gazebo_fw_dynamics_plugin.cpp.
gazebo::GazeboFwDynamicsPlugin::~GazeboFwDynamicsPlugin | ( | ) | [virtual] |
Destructor.
Definition at line 37 of file gazebo_fw_dynamics_plugin.cpp.
void gazebo::GazeboFwDynamicsPlugin::ActuatorsCallback | ( | GzActuatorsMsgPtr & | actuators_msg | ) | [private] |
Processes the actuator commands.
Converts control surface actuator inputs into physical angles before storing them and throttle values for later use in calculation of forces and moments.
Definition at line 358 of file gazebo_fw_dynamics_plugin.cpp.
void gazebo::GazeboFwDynamicsPlugin::CreatePubsAndSubs | ( | ) | [private] |
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required.
Call this once the first time OnUpdate() is called (can't be called from Load() because there is no guarantee GazeboRosInterfacePlugin has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages).
Definition at line 290 of file gazebo_fw_dynamics_plugin.cpp.
void gazebo::GazeboFwDynamicsPlugin::Load | ( | physics::ModelPtr | _model, |
sdf::ElementPtr | _sdf | ||
) | [protected] |
Called when the plugin is first created, and after the world has been loaded. This function should not be blocking.
Definition at line 40 of file gazebo_fw_dynamics_plugin.cpp.
double gazebo::GazeboFwDynamicsPlugin::NormalizedInputToAngle | ( | const ControlSurface & | surface, |
double | input | ||
) | [protected] |
Convert control surface input that is normalized in range [-1, 1] to a physical deflection angle value.
Definition at line 284 of file gazebo_fw_dynamics_plugin.cpp.
void gazebo::GazeboFwDynamicsPlugin::OnUpdate | ( | const common::UpdateInfo & | _info | ) | [protected] |
This gets called by the world update start event.
Definition at line 123 of file gazebo_fw_dynamics_plugin.cpp.
void gazebo::GazeboFwDynamicsPlugin::RollPitchYawrateThrustCallback | ( | GzRollPitchYawrateThrustMsgPtr & | roll_pitch_yawrate_thrust_msg | ) | [private] |
Process the roll_pitch_yawrate_thrust commands.
Converts the inputs into physical angles before storing them and thrust values for later use in calculation of forces and moments.
Definition at line 378 of file gazebo_fw_dynamics_plugin.cpp.
void gazebo::GazeboFwDynamicsPlugin::UpdateForcesAndMoments | ( | ) | [protected] |
Calculates the forces and moments to be applied to the fixed-wing body.
Definition at line 136 of file gazebo_fw_dynamics_plugin.cpp.
void gazebo::GazeboFwDynamicsPlugin::WindSpeedCallback | ( | GzWindSpeedMsgPtr & | wind_speed_msg | ) | [private] |
Processes the wind speed readings.
Stores the most current wind speed reading for later use in calculation of forces and moments.
Definition at line 396 of file gazebo_fw_dynamics_plugin.cpp.
gazebo::transport::SubscriberPtr gazebo::GazeboFwDynamicsPlugin::actuators_sub_ [private] |
Subscriber for receiving actuator commands.
Definition at line 105 of file gazebo_fw_dynamics_plugin.h.
Topic name for actuator commands.
Definition at line 95 of file gazebo_fw_dynamics_plugin.h.
The aerodynamic properties of the aircraft.
Definition at line 124 of file gazebo_fw_dynamics_plugin.h.
double gazebo::GazeboFwDynamicsPlugin::delta_aileron_left_ [private] |
Left aileron deflection [rad].
Definition at line 130 of file gazebo_fw_dynamics_plugin.h.
double gazebo::GazeboFwDynamicsPlugin::delta_aileron_right_ [private] |
Right aileron deflection [rad].
Definition at line 132 of file gazebo_fw_dynamics_plugin.h.
double gazebo::GazeboFwDynamicsPlugin::delta_elevator_ [private] |
Elevator deflection [rad].
Definition at line 134 of file gazebo_fw_dynamics_plugin.h.
double gazebo::GazeboFwDynamicsPlugin::delta_flap_ [private] |
Flap deflection [rad].
Definition at line 136 of file gazebo_fw_dynamics_plugin.h.
double gazebo::GazeboFwDynamicsPlugin::delta_rudder_ [private] |
Rudder deflection [rad].
Definition at line 138 of file gazebo_fw_dynamics_plugin.h.
bool gazebo::GazeboFwDynamicsPlugin::is_input_joystick_ [private] |
Are the input commands coming from a joystick (as opposed to a remote control via HIL interface, for example)?
Definition at line 77 of file gazebo_fw_dynamics_plugin.h.
physics::LinkPtr gazebo::GazeboFwDynamicsPlugin::link_ [private] |
Pointer to the link.
Definition at line 116 of file gazebo_fw_dynamics_plugin.h.
physics::ModelPtr gazebo::GazeboFwDynamicsPlugin::model_ [private] |
Pointer to the model.
Definition at line 114 of file gazebo_fw_dynamics_plugin.h.
Transport namespace.
Definition at line 93 of file gazebo_fw_dynamics_plugin.h.
transport::NodePtr gazebo::GazeboFwDynamicsPlugin::node_handle_ [private] |
Handle for the Gazebo node.
Definition at line 102 of file gazebo_fw_dynamics_plugin.h.
bool gazebo::GazeboFwDynamicsPlugin::pubs_and_subs_created_ [private] |
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from be called on every OnUpdate().
Definition at line 82 of file gazebo_fw_dynamics_plugin.h.
gazebo::transport::SubscriberPtr gazebo::GazeboFwDynamicsPlugin::roll_pitch_yawrate_thrust_sub_ [private] |
Subscriber for receiving roll_pitch_yawrate_thrust commands.
Definition at line 107 of file gazebo_fw_dynamics_plugin.h.
Topic name for roll_pitch_yawrate_thrust commands.
Definition at line 97 of file gazebo_fw_dynamics_plugin.h.
double gazebo::GazeboFwDynamicsPlugin::throttle_ [private] |
Throttle input, in range from 0 to 1.
Definition at line 140 of file gazebo_fw_dynamics_plugin.h.
Pointer to the update event connection.
Definition at line 118 of file gazebo_fw_dynamics_plugin.h.
The physical properties of the aircraft.
Definition at line 127 of file gazebo_fw_dynamics_plugin.h.
ignition::math::Vector3d gazebo::GazeboFwDynamicsPlugin::W_wind_speed_W_B_ [private] |
Most current wind speed reading [m/s].
Definition at line 121 of file gazebo_fw_dynamics_plugin.h.
gazebo::transport::SubscriberPtr gazebo::GazeboFwDynamicsPlugin::wind_speed_sub_ [private] |
Subscriber ror receiving wind speed readings.
Definition at line 109 of file gazebo_fw_dynamics_plugin.h.
Topic name for wind speed readings.
Definition at line 99 of file gazebo_fw_dynamics_plugin.h.
physics::WorldPtr gazebo::GazeboFwDynamicsPlugin::world_ [private] |
Pointer to the world.
Definition at line 112 of file gazebo_fw_dynamics_plugin.h.