32 #include <gazebo/common/Events.hh> 33 #include <gazebo/physics/physics.hh> 35 #include <geometry_msgs/WrenchStamped.h> 37 #if (GAZEBO_MAJOR_VERSION >= 8) 40 template <
typename Message,
typename T>
static inline void toVector(
const Message& msg, ignition::math::Vector3<T>& vector)
47 template <
typename Message,
typename T>
static inline void fromVector(
const ignition::math::Vector3<T>& vector, Message& msg)
54 template <
typename Message,
typename T>
static inline void toQuaternion(
const Message& msg, ignition::math::Quaternion<T>& quaternion)
56 quaternion.W() = msg.w;
57 quaternion.X() = msg.x;
58 quaternion.Y() = msg.y;
59 quaternion.Z() = msg.z;
62 template <
typename Message,
typename T>
static inline void fromQuaternion(
const ignition::math::Quaternion<T>& quaternion, Message& msg)
64 msg.w = quaternion.W();
65 msg.x = quaternion.X();
66 msg.y = quaternion.Y();
67 msg.z = quaternion.Z();
78 GazeboQuadrotorAerodynamics::GazeboQuadrotorAerodynamics()
85 #if (GAZEBO_MAJOR_VERSION < 8) 102 world = _model->GetWorld();
103 link = _model->GetLink();
113 if (_sdf->HasElement(
"robotNamespace"))
namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
114 if (_sdf->HasElement(
"paramNamespace"))
param_namespace_ = _sdf->GetElement(
"paramNamespace")->Get<std::string>();
115 if (_sdf->HasElement(
"windTopicName"))
wind_topic_ = _sdf->GetElement(
"windTopicName")->Get<std::string>();
116 if (_sdf->HasElement(
"wrenchTopic"))
wrench_topic_ = _sdf->GetElement(
"wrenchTopic")->Get<std::string>();
117 if (_sdf->HasElement(
"frameId"))
frame_id_= _sdf->GetElement(
"frameId")->Get<std::string>();
122 ROS_FATAL_STREAM(
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 123 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
131 gzwarn <<
"[quadrotor_propulsion] Could not properly configure the aerodynamics plugin. Make sure you loaded the parameter file." << std::endl;
170 #if (GAZEBO_MAJOR_VERSION >= 8) 171 Time current_time =
world->SimTime();
173 Time current_time =
world->GetSimTime();
176 last_time_ = current_time;
177 if (dt <= 0.0)
return;
183 geometry_msgs::Quaternion orientation;
184 #if (GAZEBO_MAJOR_VERSION >= 8) 185 fromQuaternion(
link->WorldPose().Rot(), orientation);
187 fromQuaternion(
link->GetWorldPose().rot, orientation);
191 geometry_msgs::Twist twist;
192 #if (GAZEBO_MAJOR_VERSION >= 8) 193 fromVector(
link->WorldLinearVel(), twist.linear);
194 fromVector(
link->WorldAngularVel(), twist.angular);
196 fromVector(
link->GetWorldLinearVel(), twist.linear);
197 fromVector(
link->GetWorldAngularVel(), twist.angular);
205 #if (GAZEBO_MAJOR_VERSION >= 8) 206 ignition::math::Vector3d force, torque;
208 math::Vector3 force, torque;
215 geometry_msgs::WrenchStamped wrench_msg;
216 wrench_msg.header.stamp =
ros::Time(current_time.sec, current_time.nsec);
223 link->AddRelativeForce(force);
224 #if (GAZEBO_MAJOR_VERSION >= 8) 225 link->AddRelativeTorque(torque -
link->GetInertial()->CoG().Cross(force));
227 link->AddRelativeTorque(torque -
link->GetInertial()->GetCoG().Cross(force));
242 static const double timeout = 0.01;
void initByFullCallbackType(const std::string &_topic, uint32_t _queue_size, const boost::function< void(P)> &_callback, const boost::function< boost::shared_ptr< typename ParameterAdapter< P >::Message >(void)> &factory_fn=DefaultMessageCreator< typename ParameterAdapter< P >::Message >())
boost::thread callback_queue_thread_
void publish(const boost::shared_ptr< M > &message) const
void setWind(const geometry_msgs::Vector3 &wind)
void init(const std::string &_topic, uint32_t _queue_size, const SubscriberStatusCallback &_connect_cb=SubscriberStatusCallback(), const SubscriberStatusCallback &_disconnect_cb=SubscriberStatusCallback())
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual ~GazeboQuadrotorAerodynamics()
ROSCPP_DECL bool isInitialized()
const geometry_msgs::Wrench & getWrench() const
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
ros::CallbackQueue callback_queue_
void setOrientation(const geometry_msgs::Quaternion &orientation)
void setTwist(const geometry_msgs::Twist &twist)
ros::Publisher wrench_publisher_
ros::Subscriber wind_subscriber_
std::string wrench_topic_
static void fromVector(const Vector &vector, Message &msg)
bool configure(const ros::NodeHandle ¶m=ros::NodeHandle("~"))
#define ROS_FATAL_STREAM(args)
physics::LinkPtr link
The link referred to by this plugin.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string param_namespace_
static void fromQuaternion(const Quaternion &quaternion, Message &msg)
ros::NodeHandle * node_handle_
static void toQuaternion(const Message &msg, Quaternion &quaternion)
physics::WorldPtr world
The parent World.
hector_quadrotor_model::QuadrotorAerodynamics model_
CallbackQueueInterface * callback_queue
static void toVector(const Message &msg, Vector &vector)
event::ConnectionPtr updateConnection
CallbackQueueInterface * callback_queue