20 #include "ConnectRosToGazeboTopic.pb.h" 27 W_wind_speed_W_B_(0, 0, 0),
28 delta_aileron_left_(0.0),
29 delta_aileron_right_(0.0),
34 pubs_and_subs_created_(false) {
41 sdf::ElementPtr _sdf) {
43 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
46 gzdbg <<
"_model = " << _model->GetName() << std::endl;
55 if (_sdf->HasElement(
"robotNamespace"))
56 namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
58 gzerr <<
"[gazebo_fw_dynamics_plugin] Please specify a robotNamespace.\n";
61 node_handle_ = transport::NodePtr(
new transport::Node());
67 std::string link_name;
68 if (_sdf->HasElement(
"linkName"))
69 link_name = _sdf->GetElement(
"linkName")->Get<std::string>();
71 gzerr <<
"[gazebo_fw_dynamics_plugin] Please specify a linkName.\n";
75 gzthrow(
"[gazebo_fw_dynamics_plugin] Couldn't find specified link \"" 76 << link_name <<
"\".");
81 if (_sdf->HasElement(
"aeroParamsYAML")) {
82 std::string aero_params_yaml =
83 _sdf->GetElement(
"aeroParamsYAML")->Get<std::string>();
87 gzwarn <<
"[gazebo_fw_dynamics_plugin] No aerodynamic paramaters YAML file" 88 <<
" specified, using default Techpod parameters.\n";
93 if (_sdf->HasElement(
"vehicleParamsYAML")) {
94 std::string vehicle_params_yaml =
95 _sdf->GetElement(
"vehicleParamsYAML")->Get<std::string>();
99 gzwarn <<
"[gazebo_fw_dynamics_plugin] No vehicle paramaters YAML file" 100 <<
" specified, using default Techpod parameters.\n";
106 getSdfParam<std::string>(_sdf,
"actuatorsSubTopic",
109 getSdfParam<std::string>(_sdf,
"rollPitchYawrateThrustSubTopic",
113 getSdfParam<std::string>(_sdf,
"windSpeedSubTopic",
125 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
140 ignition::math::Quaterniond W_rot_W_B =
link_->WorldPose().Rot();
141 ignition::math::Vector3d B_air_speed_W_B = W_rot_W_B.RotateVectorReverse(
143 ignition::math::Vector3d B_angular_velocity_W_B =
link_->RelativeAngularVel();
148 double u = B_air_speed_W_B.X();
149 double v = -B_air_speed_W_B.Y();
150 double w = -B_air_speed_W_B.Z();
152 double p = B_angular_velocity_W_B.X();
153 double q = -B_angular_velocity_W_B.Y();
154 double r = -B_angular_velocity_W_B.Z();
159 double V = B_air_speed_W_B.Length();
176 double flap_diff = 0.0;
179 const double drag = q_bar_S *
181 Eigen::Vector3d(1.0, alpha, alpha * alpha)) +
183 Eigen::Vector3d(0.0, beta, beta * beta)) +
185 Eigen::Vector3d(0.0, aileron_sum, aileron_sum * aileron_sum)) +
187 Eigen::Vector3d(0.0, flap_sum, flap_sum * flap_sum)));
189 const double side_force = q_bar_S *
191 Eigen::Vector2d(0.0, beta)));
193 const double lift = q_bar_S *
195 Eigen::Vector4d(1.0, alpha, alpha * alpha, alpha * alpha * alpha)) +
197 Eigen::Vector2d(0.0, aileron_sum)) +
199 Eigen::Vector2d(0.0, flap_sum)));
201 const Eigen::Vector3d forces_Wind(-drag, side_force, -lift);
216 Eigen::Vector2d(0.0, beta)) +
218 Eigen::Vector2d(0.0, p_hat)) +
220 Eigen::Vector2d(0.0, r_hat)) +
222 Eigen::Vector2d(0.0, aileron_diff)) +
224 Eigen::Vector2d(0.0, flap_diff)));
228 Eigen::Vector2d(1.0, alpha)) +
230 Eigen::Vector2d(0.0, q_hat)) +
236 Eigen::Vector2d(0.0, beta)) +
238 Eigen::Vector2d(0.0, r_hat)) +
242 const Eigen::Vector3d moments_Wind(rolling_moment,
250 const Eigen::Vector3d force_thrust_B = thrust * Eigen::Vector3d(
256 double ca =
cos(alpha);
257 double sa =
sin(alpha);
258 double cb =
cos(beta);
259 double sb =
sin(beta);
261 Eigen::Matrix3d R_Wind_B;
262 R_Wind_B << ca * cb, sb, sa * cb,
263 -sb * ca, cb, -sa * sb,
266 const Eigen::Matrix3d R_Wind_B_t = R_Wind_B.transpose();
269 const Eigen::Vector3d forces_B = R_Wind_B_t * forces_Wind + force_thrust_B;
270 const Eigen::Vector3d moments_B = R_Wind_B_t * moments_Wind;
274 const ignition::math::Vector3d forces =
275 ignition::math::Vector3d (forces_B[0], -forces_B[1], -forces_B[2]);
276 const ignition::math::Vector3d moments =
277 ignition::math::Vector3d (moments_B[0], -moments_B[1], -moments_B[2]);
280 link_->AddRelativeForce(forces);
281 link_->AddRelativeTorque(moments);
291 gzdbg << __PRETTY_FUNCTION__ <<
" called." << std::endl;
294 gazebo::transport::PublisherPtr gz_connect_ros_to_gazebo_topic_pub =
295 node_handle_->Advertise<gz_std_msgs::ConnectRosToGazeboTopic>(
297 gz_std_msgs::ConnectRosToGazeboTopic connect_ros_to_gazebo_topic_msg;
309 connect_ros_to_gazebo_topic_msg.set_ros_topic(
namespace_ +
"/" +
311 connect_ros_to_gazebo_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
313 connect_ros_to_gazebo_topic_msg.set_msgtype(
314 gz_std_msgs::ConnectRosToGazeboTopic::WIND_SPEED);
315 gz_connect_ros_to_gazebo_topic_pub->Publish(connect_ros_to_gazebo_topic_msg,
330 connect_ros_to_gazebo_topic_msg.set_ros_topic(
namespace_ +
"/" +
332 connect_ros_to_gazebo_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
334 connect_ros_to_gazebo_topic_msg.set_msgtype(
335 gz_std_msgs::ConnectRosToGazeboTopic::ROLL_PITCH_YAWRATE_THRUST);
346 connect_ros_to_gazebo_topic_msg.set_ros_topic(
namespace_ +
"/" +
348 connect_ros_to_gazebo_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
350 connect_ros_to_gazebo_topic_msg.set_msgtype(
351 gz_std_msgs::ConnectRosToGazeboTopic::ACTUATORS);
354 gz_connect_ros_to_gazebo_topic_pub->Publish(connect_ros_to_gazebo_topic_msg,
361 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
381 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
385 roll_pitch_yawrate_thrust_msg->roll());
387 roll_pitch_yawrate_thrust_msg->roll());
389 roll_pitch_yawrate_thrust_msg->pitch());
391 roll_pitch_yawrate_thrust_msg->yaw_rate());
393 throttle_ = roll_pitch_yawrate_thrust_msg->thrust().x();
399 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
Eigen::Vector2d c_roll_moment_p
void CreatePubsAndSubs()
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required...
Eigen::Vector3d c_drag_alpha
physics::LinkPtr link_
Pointer to the link.
Eigen::Vector2d c_side_force_beta
std::string actuators_sub_topic_
Topic name for actuator commands.
void LoadVehicleParamsYAML(const std::string &yaml_path)
Eigen::Vector2d c_roll_moment_delta_flp
ControlSurface aileron_right
transport::NodePtr node_handle_
Handle for the Gazebo node.
static constexpr char WIND_SPEED[]
static const std::string kConnectRosToGazeboSubtopic
static constexpr bool kDefaultIsInputJoystick
static constexpr double kAirDensity
void ActuatorsCallback(GzActuatorsMsgPtr &actuators_msg)
Processes the actuator commands.
Eigen::Vector3d c_drag_delta_ail
Eigen::Vector2d c_roll_moment_r
double NormalizedInputToAngle(const ControlSurface &surface, double input)
Convert control surface input that is normalized in range [-1, 1] to a physical deflection angle valu...
static constexpr char COMMAND_ROLL_PITCH_YAWRATE_THRUST[]
double throttle_
Throttle input, in range from 0 to 1.
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
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 no...
gazebo::transport::SubscriberPtr wind_speed_sub_
Subscriber ror receiving wind speed readings.
double delta_aileron_left_
Left aileron deflection [rad].
static constexpr char COMMAND_ACTUATORS[]
ignition::math::Vector3d W_wind_speed_W_B_
Most current wind speed reading [m/s].
void OnUpdate(const common::UpdateInfo &)
This gets called by the world update start event.
std::string roll_pitch_yawrate_thrust_sub_topic_
Topic name for roll_pitch_yawrate_thrust commands.
Eigen::Vector3d c_drag_beta
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
FWVehicleParameters vehicle_params_
The physical properties of the aircraft.
Eigen::Vector2d c_roll_moment_delta_ail
Eigen::Vector2d c_pitch_moment_q
Eigen::Vector2d c_pitch_moment_delta_elv
Eigen::Vector3d c_drag_delta_flp
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
Eigen::Vector2d c_yaw_moment_beta
static const bool kPrintOnPluginLoad
std::string namespace_
Transport namespace.
physics::ModelPtr model_
Pointer to the model.
Eigen::Vector2d c_roll_moment_beta
void LoadAeroParamsYAML(const std::string &yaml_path)
void UpdateForcesAndMoments()
Calculates the forces and moments to be applied to the fixed-wing body.
Eigen::Vector4d c_lift_alpha
Eigen::Vector2d c_lift_delta_ail
ControlSurface aileron_left
Eigen::Vector2d c_lift_delta_flp
gazebo::transport::SubscriberPtr actuators_sub_
Subscriber for receiving actuator commands.
static const bool kPrintOnUpdates
double delta_rudder_
Rudder deflection [rad].
Eigen::Vector2d c_yaw_moment_delta_rud
double delta_flap_
Flap deflection [rad].
double thrust_inclination
void WindSpeedCallback(GzWindSpeedMsgPtr &wind_speed_msg)
Processes the wind speed readings.
void RollPitchYawrateThrustCallback(GzRollPitchYawrateThrustMsgPtr &roll_pitch_yawrate_thrust_msg)
Process the roll_pitch_yawrate_thrust commands.
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
bool is_input_joystick_
Are the input commands coming from a joystick (as opposed to a remote control via HIL interface...
physics::WorldPtr world_
Pointer to the world.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
FWAerodynamicParameters aero_params_
The aerodynamic properties of the aircraft.
Eigen::Vector2d c_yaw_moment_r
std::string wind_speed_sub_topic_
Topic name for wind speed readings.
Eigen::Vector2d c_pitch_moment_alpha
gazebo::transport::SubscriberPtr roll_pitch_yawrate_thrust_sub_
Subscriber for receiving roll_pitch_yawrate_thrust commands.
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
static const bool kPrintOnMsgCallback
static constexpr double kMinAirSpeedThresh
double delta_elevator_
Elevator deflection [rad].
GazeboFwDynamicsPlugin()
Constructor.
virtual ~GazeboFwDynamicsPlugin()
Destructor.
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
double delta_aileron_right_
Right aileron deflection [rad].