30 #include <gazebo/common/Events.hh> 31 #include <gazebo/physics/physics.hh> 35 #include <geometry_msgs/Wrench.h> 47 #if (GAZEBO_MAJOR_VERSION < 8) 60 gzwarn <<
"The GazeboQuadrotorSimpleController plugin is DEPRECATED in ROS hydro. Please use the twist_controller in package hector_quadrotor_controller instead." << std::endl;
62 world = _model->GetWorld();
63 link = _model->GetLink();
76 if (_sdf->HasElement(
"robotNamespace"))
namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
77 if (_sdf->HasElement(
"topicName"))
velocity_topic_ = _sdf->GetElement(
"topicName")->Get<std::string>();
78 if (_sdf->HasElement(
"imuTopic"))
imu_topic_ = _sdf->GetElement(
"imuTopic")->Get<std::string>();
79 if (_sdf->HasElement(
"stateTopic"))
state_topic_ = _sdf->GetElement(
"stateTopic")->Get<std::string>();
80 if (_sdf->HasElement(
"wrenchTopic"))
wrench_topic_ = _sdf->GetElement(
"wrenchTopic")->Get<std::string>();
81 if (_sdf->HasElement(
"maxForce"))
max_force_ = _sdf->GetElement(
"maxForce")->Get<
double>();
82 if (_sdf->HasElement(
"autoEngage"))
auto_engage_ = _sdf->GetElement(
"autoEngage")->Get<
bool>();
84 if (_sdf->HasElement(
"bodyName") && _sdf->GetElement(
"bodyName")->GetValue()) {
85 link_name_ = _sdf->GetElement(
"bodyName")->Get<std::string>();
91 ROS_FATAL(
"gazebo_ros_baro plugin error: bodyName: %s does not exist\n",
link_name_.c_str());
104 #if (GAZEBO_MAJOR_VERSION >= 8) 108 inertia =
link->GetInertial()->GetPrincipalMoments();
109 mass =
link->GetInertial()->GetMass();
115 ROS_FATAL_STREAM(
"A ROS node for Gazebo has not been initialized, unable to load plugin. " 116 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
144 ROS_INFO_NAMED(
"quadrotor_simple_controller",
"Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
157 ROS_INFO_NAMED(
"quadrotor_simple_controller",
"Using state information on topic %s as source of state information.", state_topic_.c_str());
179 ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
208 #if (GAZEBO_MAJOR_VERSION >= 8) 209 pose.Rot().Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
211 angular_velocity =
pose.Rot().RotateVector(ignition::math::Vector3d(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z));
213 pose.rot.Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
215 angular_velocity =
pose.rot.RotateVector(math::Vector3(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z));
221 #if (GAZEBO_MAJOR_VERSION >= 8) 222 ignition::math::Vector3d velocity1(
velocity);
228 #if (GAZEBO_MAJOR_VERSION >= 8) 229 pose.Pos().Set(state->pose.pose.position.x, state->pose.pose.position.y, state->pose.pose.position.z);
230 pose.Rot().Set(state->pose.pose.orientation.w, state->pose.pose.orientation.x, state->pose.pose.orientation.y, state->pose.pose.orientation.z);
233 pose.pos.Set(state->pose.pose.position.x, state->pose.pose.position.y, state->pose.pose.position.z);
234 pose.rot.Set(state->pose.pose.orientation.w, state->pose.pose.orientation.x, state->pose.pose.orientation.y, state->pose.pose.orientation.z);
237 angular_velocity.Set(state->twist.twist.angular.x, state->twist.twist.angular.y, state->twist.twist.angular.z);
240 velocity.Set(state->twist.twist.linear.x, state->twist.twist.linear.y, state->twist.twist.linear.z);
254 ROS_INFO_NAMED(
"quadrotor_simple_controller",
"Engaging motors!");
261 ROS_INFO_NAMED(
"quadrotor_simple_controller",
"Shutting down motors!");
276 #if (GAZEBO_MAJOR_VERSION >= 8) 302 ROS_INFO_NAMED(
"quadrotor_simple_controller",
"Engaging motors!");
303 #if (GAZEBO_MAJOR_VERSION >= 8) 309 ROS_INFO_NAMED(
"quadrotor_simple_controller",
"Shutting down motors!");
322 #if (GAZEBO_MAJOR_VERSION >= 8) 323 ignition::math::Vector3d gravity_body =
pose.Rot().RotateVector(
world->Gravity());
324 double gravity = gravity_body.Length();
325 double load_factor = gravity * gravity /
world->Gravity().Dot(gravity_body);
327 math::Vector3 gravity_body =
pose.rot.RotateVector(
world->GetPhysicsEngine()->GetGravity());
328 double gravity = gravity_body.GetLength();
329 double load_factor = gravity * gravity /
world->GetPhysicsEngine()->GetGravity().Dot(gravity_body);
333 #if (GAZEBO_MAJOR_VERSION >= 8) 334 ignition::math::Quaterniond heading_quaternion(cos(
euler.Z()/2),0,0,sin(
euler.Z()/2));
335 ignition::math::Vector3d velocity_xy = heading_quaternion.RotateVectorReverse(
velocity);
336 ignition::math::Vector3d acceleration_xy = heading_quaternion.RotateVectorReverse(
acceleration);
337 ignition::math::Vector3d angular_velocity_body =
pose.Rot().RotateVectorReverse(
angular_velocity);
339 math::Quaternion heading_quaternion(cos(
euler.z/2),0,0,sin(
euler.z/2));
340 math::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(
velocity);
341 math::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(
acceleration);
346 force.Set(0.0, 0.0, 0.0);
347 torque.Set(0.0, 0.0, 0.0);
349 #if (GAZEBO_MAJOR_VERSION >= 8) 392 geometry_msgs::Wrench wrench;
393 #if (GAZEBO_MAJOR_VERSION >= 8) 394 wrench.force.x =
force.X();
395 wrench.force.y =
force.Y();
396 wrench.force.z =
force.Z();
397 wrench.torque.x =
torque.X();
398 wrench.torque.y =
torque.Y();
399 wrench.torque.z =
torque.Z();
401 wrench.force.x =
force.x;
402 wrench.force.y =
force.y;
403 wrench.force.z =
force.z;
404 wrench.torque.x =
torque.x;
405 wrench.torque.y =
torque.y;
406 wrench.torque.z =
torque.z;
414 #if (GAZEBO_MAJOR_VERSION >= 8) 466 if (_sdf->HasElement(prefix +
"ProportionalGain")) gain_p = _sdf->GetElement(prefix +
"ProportionalGain")->Get<
double>();
467 if (_sdf->HasElement(prefix +
"DifferentialGain")) gain_d = _sdf->GetElement(prefix +
"DifferentialGain")->Get<
double>();
468 if (_sdf->HasElement(prefix +
"IntegralGain")) gain_i = _sdf->GetElement(prefix +
"IntegralGain")->Get<
double>();
469 if (_sdf->HasElement(prefix +
"TimeConstant")) time_constant = _sdf->GetElement(prefix +
"TimeConstant")->Get<
double>();
470 if (_sdf->HasElement(prefix +
"Limit"))
limit = _sdf->GetElement(prefix +
"Limit")->Get<
double>();
476 if (
limit > 0.0 && fabs(new_input) >
limit) new_input = (new_input < 0 ? -1.0 : 1.0) *
limit;
479 if (dt + time_constant > 0.0) {
480 dinput = (new_input - input) / (dt + time_constant);
481 input = (dt * new_input + time_constant * input) / (dt + time_constant);
490 output = gain_p * p + gain_d *
d + gain_i * i;
497 p = i =
d = output = 0;
physics::LinkPtr link
The link referred to by this plugin.
boost::shared_ptr< void const > VoidConstPtr
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
#define ROS_INFO_NAMED(name,...)
virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf, const std::string &_prefix="update")
double update(double input, double x, double dx, double dt)
void publish(const boost::shared_ptr< M > &message) const
struct gazebo::GazeboQuadrotorSimpleController::Controllers controllers_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL bool isInitialized()
ros::Subscriber imu_subscriber_
std::string velocity_topic_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void ImuCallback(const sensor_msgs::ImuConstPtr &)
virtual ~GazeboQuadrotorSimpleController()
math::Vector3 angular_velocity
virtual void Load(sdf::ElementPtr _sdf, const std::string &prefix="")
ros::ServiceServer shutdown_service_server_
ros::CallbackQueue callback_queue_
void VelocityCallback(const geometry_msgs::TwistConstPtr &)
ros::NodeHandle * node_handle_
bool EngageCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
#define ROS_FATAL_STREAM(args)
math::Vector3 acceleration
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
GazeboQuadrotorSimpleController()
void limit(T &value, const T &min, const T &max)
std::string wrench_topic_
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
event::ConnectionPtr updateConnection
bool getParam(const std::string &key, std::string &s) const
bool ShutdownCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
ros::Subscriber state_subscriber_
void StateCallback(const nav_msgs::OdometryConstPtr &)
physics::WorldPtr world
The parent World.
boost::shared_ptr< void > VoidPtr
ros::Publisher wrench_publisher_
ros::ServiceServer engage_service_server_
geometry_msgs::Twist velocity_command_
ros::Subscriber velocity_subscriber_