32 #include <gazebo/common/Events.hh> 33 #include <gazebo/physics/physics.hh> 35 #include <rosgraph_msgs/Clock.h> 36 #include <geometry_msgs/WrenchStamped.h> 38 #if (GAZEBO_MAJOR_VERSION >= 8) 41 template <
typename Message,
typename T>
static inline void toVector(
const Message& msg, ignition::math::Vector3<T>& vector)
48 template <
typename Message,
typename T>
static inline void fromVector(
const ignition::math::Vector3<T>& vector, Message& msg)
55 template <
typename Message,
typename T>
static inline void toQuaternion(
const Message& msg, ignition::math::Quaternion<T>& quaternion)
57 quaternion.W() = msg.w;
58 quaternion.X() = msg.x;
59 quaternion.Y() = msg.y;
60 quaternion.Z() = msg.z;
63 template <
typename Message,
typename T>
static inline void fromQuaternion(
const ignition::math::Quaternion<T>& quaternion, Message& msg)
65 msg.w = quaternion.W();
66 msg.x = quaternion.X();
67 msg.y = quaternion.Y();
68 msg.z = quaternion.Z();
79 GazeboQuadrotorPropulsion::GazeboQuadrotorPropulsion()
86 #if (GAZEBO_MAJOR_VERSION < 8) 103 world = _model->GetWorld();
104 link = _model->GetLink();
120 if (_sdf->HasElement(
"robotNamespace"))
namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
121 if (_sdf->HasElement(
"paramNamespace"))
param_namespace_ = _sdf->GetElement(
"paramNamespace")->Get<std::string>();
122 if (_sdf->HasElement(
"triggerTopic"))
trigger_topic_ = _sdf->GetElement(
"triggerTopic")->Get<std::string>();
123 if (_sdf->HasElement(
"topicName"))
command_topic_ = _sdf->GetElement(
"topicName")->Get<std::string>();
124 if (_sdf->HasElement(
"pwmTopicName"))
pwm_topic_ = _sdf->GetElement(
"pwmTopicName")->Get<std::string>();
125 if (_sdf->HasElement(
"wrenchTopic"))
wrench_topic_ = _sdf->GetElement(
"wrenchTopic")->Get<std::string>();
126 if (_sdf->HasElement(
"supplyTopic"))
supply_topic_ = _sdf->GetElement(
"supplyTopic")->Get<std::string>();
127 if (_sdf->HasElement(
"statusTopic"))
status_topic_ = _sdf->GetElement(
"statusTopic")->Get<std::string>();
128 if (_sdf->HasElement(
"frameId"))
frame_id_= _sdf->GetElement(
"frameId")->Get<std::string>();
130 if (_sdf->HasElement(
"voltageTopicName")) {
131 gzwarn <<
"[quadrotor_propulsion] Plugin parameter 'voltageTopicName' is deprecated! Plese change your config to use " 132 <<
"'topicName' for MotorCommand messages or 'pwmTopicName' for MotorPWM messages." << std::endl;
133 pwm_topic_ = _sdf->GetElement(
"voltageTopicName")->Get<std::string>();
139 if (_sdf->HasElement(
"controlTolerance"))
control_tolerance_.
fromSec(_sdf->GetElement(
"controlTolerance")->Get<
double>());
140 if (_sdf->HasElement(
"controlDelay"))
control_delay_.
fromSec(_sdf->GetElement(
"controlDelay")->Get<
double>());
148 ROS_FATAL_STREAM(
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 149 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
157 gzwarn <<
"[quadrotor_propulsion] Could not properly configure the propulsion plugin. Make sure you loaded the parameter file." << std::endl;
233 #if (GAZEBO_MAJOR_VERSION >= 8) 234 Time current_time =
world->SimTime();
236 Time current_time =
world->GetSimTime();
239 last_time_ = current_time;
240 if (dt <= 0.0)
return;
245 rosgraph_msgs::Clock clock;
246 clock.clock =
ros::Time(current_time.sec, current_time.nsec);
260 geometry_msgs::Twist twist;
261 #if (GAZEBO_MAJOR_VERSION >= 8) 262 fromVector(
link->RelativeLinearVel(), twist.linear);
263 fromVector(
link->RelativeAngularVel(), twist.angular);
265 fromVector(
link->GetRelativeLinearVel(), twist.linear);
266 fromVector(
link->GetRelativeAngularVel(), twist.angular);
274 #if (GAZEBO_MAJOR_VERSION >= 8) 275 ignition::math::Vector3d force, torque;
277 math::Vector3 force, torque;
284 geometry_msgs::WrenchStamped wrench_msg;
285 wrench_msg.header.stamp =
ros::Time(current_time.sec, current_time.nsec);
294 motor_status.header.stamp =
ros::Time(current_time.sec, current_time.nsec);
306 link->AddRelativeForce(force);
307 #if (GAZEBO_MAJOR_VERSION >= 8) 308 link->AddRelativeTorque(torque -
link->GetInertial()->CoG().Cross(force));
310 link->AddRelativeTorque(torque -
link->GetInertial()->GetCoG().Cross(force));
329 static const double timeout = 0.01;
bool configure(const ros::NodeHandle ¶m=ros::NodeHandle("~"))
hector_quadrotor_model::QuadrotorPropulsion model_
const hector_uav_msgs::MotorStatus & getMotorStatus() const
virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf, const std::string &_prefix="update")
#define ROS_DEBUG_STREAM_NAMED(name, args)
void publish(const boost::shared_ptr< M > &message) const
ros::Duration control_delay_
bool processQueue(const ros::Time ×tamp, const ros::Duration &tolerance=ros::Duration(), const ros::Duration &delay=ros::Duration(), const ros::WallDuration &wait=ros::WallDuration(), ros::CallbackQueue *callback_queue=0)
void init(const std::string &_topic, uint32_t _queue_size, const SubscriberStatusCallback &_connect_cb=SubscriberStatusCallback(), const SubscriberStatusCallback &_disconnect_cb=SubscriberStatusCallback())
ros::Publisher trigger_publisher_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
UpdateTimer motorStatusTimer
ROSCPP_DECL bool isInitialized()
const geometry_msgs::Wrench & getWrench() const
std::string param_namespace_
ros::Publisher wrench_publisher_
const hector_uav_msgs::Supply & getSupply() const
common::Time last_trigger_time_
physics::LinkPtr link
The link referred to by this plugin.
ros::Publisher motor_status_publisher_
static void fromVector(const Vector &vector, Message &msg)
common::Time const & getUpdatePeriod() const
#define ROS_FATAL_STREAM(args)
Duration & fromSec(double t)
std::string command_topic_
ros::Subscriber pwm_subscriber_
void addPWMToQueue(const hector_uav_msgs::MotorPWMConstPtr &pwm)
ros::Duration control_tolerance_
event::ConnectionPtr updateConnection
std::string status_topic_
virtual ~GazeboQuadrotorPropulsion()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
std::string trigger_topic_
ros::NodeHandle * node_handle_
ros::Subscriber command_subscriber_
common::Time last_motor_status_time_
std::string wrench_topic_
static void fromQuaternion(const Quaternion &quaternion, Message &msg)
physics::WorldPtr world
The parent World.
static void toQuaternion(const Message &msg, Quaternion &quaternion)
ros::Publisher supply_publisher_
void init(const std::string &_topic, uint32_t _queue_size, const boost::function< void(const boost::shared_ptr< M const > &)> &_callback, const boost::function< boost::shared_ptr< M >(void)> &factory_fn=DefaultMessageCreator< M >())
CallbackQueueInterface * callback_queue
void setInitialSupplyVoltage(double voltage)
static void toVector(const Message &msg, Vector &vector)
ros::CallbackQueue callback_queue_
boost::thread callback_queue_thread_
std::string supply_topic_
CallbackQueueInterface * callback_queue
common::Time last_supply_time_
void setTwist(const geometry_msgs::Twist &twist)
void addCommandToQueue(const hector_uav_msgs::MotorCommandConstPtr &command)