21 #ifndef _GAZEBO_GIMBAL_CONTROLLER_PLUGIN_HH_ 22 #define _GAZEBO_GIMBAL_CONTROLLER_PLUGIN_HH_ 27 #include <gazebo/common/PID.hh> 28 #include <gazebo/common/Plugin.hh> 29 #include <gazebo/physics/physics.hh> 30 #include <gazebo/transport/transport.hh> 31 #include <gazebo/util/system.hh> 32 #include <gazebo/sensors/sensors.hh> 41 public:
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
43 public:
virtual void Init();
45 private:
void OnUpdate();
47 private:
void OnPitchStringMsg(ConstAnyPtr &_msg);
48 private:
void OnRollStringMsg(ConstAnyPtr &_msg);
49 private:
void OnYawStringMsg(ConstAnyPtr &_msg);
57 private:
double NormalizeAbout(
double _angle,
double _reference);
64 private:
double ShortestAngularDistance(
double _from,
double _to);
66 private: sdf::ElementPtr
sdf;
71 private: transport::SubscriberPtr
rollSub;
72 private: transport::SubscriberPtr
yawSub;
76 private: transport::PublisherPtr
yawPub;
78 private: physics::ModelPtr
model;
97 private: transport::NodePtr
node;
104 private: ignition::ignition::math::Vector3d ThreeAxisRot(
105 double r11,
double r12,
double r21,
double r31,
double r32);
106 private: ignition::ignition::math::Vector3d QtoZXY(
107 const ignition::math::Quaterniond &_q);
transport::SubscriberPtr rollSub
transport::SubscriberPtr pitchSub
physics::JointPtr yawJoint
yaw camera
sensors::ImuSensorPtr imuSensor
physics::JointPtr pitchJoint
camera pitch joint
transport::PublisherPtr pitchPub
transport::PublisherPtr yawPub
physics::JointPtr rollJoint
camera roll joint
transport::SubscriberPtr yawSub
std::vector< event::ConnectionPtr > connections
common::Time lastUpdateTime
transport::PublisherPtr rollPub