Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H
00018 #define ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H
00019
00020
00021 #include <random>
00022
00023 #include <Eigen/Core>
00024 #include <gazebo/common/Plugin.hh>
00025 #include <gazebo/common/common.hh>
00026 #include <gazebo/gazebo.hh>
00027 #include <gazebo/physics/physics.hh>
00028 #include "gazebo/msgs/msgs.hh"
00029
00030
00031 #include <mav_msgs/default_topics.h>
00032 #include <ros/callback_queue.h>
00033 #include <ros/ros.h>
00034 #include <tf/transform_broadcaster.h>
00035
00036
00037 #include "ConnectGazeboToRosTopic.pb.h"
00038 #include "ConnectRosToGazeboTopic.pb.h"
00039
00040 #include "Actuators.pb.h"
00041 #include "CommandMotorSpeed.pb.h"
00042 #include "Float32.pb.h"
00043 #include "FluidPressure.pb.h"
00044 #include "Imu.pb.h"
00045 #include "JointState.pb.h"
00046 #include "MagneticField.pb.h"
00047 #include "NavSatFix.pb.h"
00048 #include "Odometry.pb.h"
00049 #include "PoseWithCovarianceStamped.pb.h"
00050 #include "RollPitchYawrateThrust.pb.h"
00051 #include "TransformStamped.pb.h"
00052 #include "TransformStampedWithFrameIds.pb.h"
00053 #include "TwistStamped.pb.h"
00054 #include "Vector3dStamped.pb.h"
00055 #include "WindSpeed.pb.h"
00056 #include "WrenchStamped.pb.h"
00057
00058
00059 #include <geometry_msgs/Point.h>
00060 #include <geometry_msgs/PointStamped.h>
00061 #include <geometry_msgs/Pose.h>
00062 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00063 #include <geometry_msgs/TransformStamped.h>
00064 #include <geometry_msgs/TwistStamped.h>
00065 #include <geometry_msgs/WrenchStamped.h>
00066 #include <mav_msgs/Actuators.h>
00067 #include <mav_msgs/RollPitchYawrateThrust.h>
00068 #include <nav_msgs/Odometry.h>
00069 #include <rotors_comm/WindSpeed.h>
00070 #include <sensor_msgs/FluidPressure.h>
00071 #include <sensor_msgs/Imu.h>
00072 #include <sensor_msgs/JointState.h>
00073 #include <sensor_msgs/MagneticField.h>
00074 #include <sensor_msgs/NavSatFix.h>
00075 #include <std_msgs/Float32.h>
00076
00077 #include "rotors_gazebo_plugins/common.h"
00078
00079 namespace gazebo {
00080
00081
00082 typedef const boost::shared_ptr<const gz_std_msgs::ConnectGazeboToRosTopic>
00083 GzConnectGazeboToRosTopicMsgPtr;
00084 typedef const boost::shared_ptr<const gz_std_msgs::ConnectRosToGazeboTopic>
00085 GzConnectRosToGazeboTopicMsgPtr;
00086 typedef const boost::shared_ptr<const gz_std_msgs::Float32> GzFloat32MsgPtr;
00087 typedef const boost::shared_ptr<const gz_geometry_msgs::Odometry>
00088 GzOdometryMsgPtr;
00089 typedef const boost::shared_ptr<const gazebo::msgs::Pose> GzPoseMsgPtr;
00090 typedef const boost::shared_ptr<
00091 const gz_geometry_msgs::PoseWithCovarianceStamped>
00092 GzPoseWithCovarianceStampedMsgPtr;
00093 typedef const boost::shared_ptr<const gz_geometry_msgs::TransformStamped>
00094 GzTransformStampedMsgPtr;
00095 typedef const boost::shared_ptr<
00096 const gz_geometry_msgs::TransformStampedWithFrameIds>
00097 GzTransformStampedWithFrameIdsMsgPtr;
00098 typedef const boost::shared_ptr<const gz_geometry_msgs::TwistStamped>
00099 GzTwistStampedMsgPtr;
00100 typedef const boost::shared_ptr<const gz_geometry_msgs::Vector3dStamped>
00101 GzVector3dStampedMsgPtr;
00102 typedef const boost::shared_ptr<const gz_geometry_msgs::WrenchStamped>
00103 GzWrenchStampedMsgPtr;
00104 typedef const boost::shared_ptr<const gz_mav_msgs::RollPitchYawrateThrust>
00105 GzRollPitchYawrateThrustPtr;
00106 typedef const boost::shared_ptr<const gz_mav_msgs::WindSpeed> GzWindSpeedMsgPtr;
00107 typedef const boost::shared_ptr<const gz_sensor_msgs::Actuators>
00108 GzActuatorsMsgPtr;
00109 typedef const boost::shared_ptr<const gz_sensor_msgs::FluidPressure>
00110 GzFluidPressureMsgPtr;
00111 typedef const boost::shared_ptr<const gz_sensor_msgs::Imu> GzImuPtr;
00112 typedef const boost::shared_ptr<const gz_sensor_msgs::JointState>
00113 GzJointStateMsgPtr;
00114 typedef const boost::shared_ptr<const gz_sensor_msgs::MagneticField>
00115 GzMagneticFieldMsgPtr;
00116 typedef const boost::shared_ptr<const gz_sensor_msgs::NavSatFix> GzNavSatFixPtr;
00117
00123 class GazeboRosInterfacePlugin : public WorldPlugin {
00124 public:
00125 GazeboRosInterfacePlugin();
00126 ~GazeboRosInterfacePlugin();
00127
00128 void InitializeParams();
00129 void Publish();
00130
00131 protected:
00132 void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf);
00133
00136 void OnUpdate(const common::UpdateInfo&);
00137
00138 private:
00145 template <typename GazeboMsgT, typename RosMsgT>
00146 void ConnectHelper(void (GazeboRosInterfacePlugin::*fp)(
00147 const boost::shared_ptr<GazeboMsgT const>&,
00148 ros::Publisher),
00149 GazeboRosInterfacePlugin* ptr, std::string gazeboNamespace,
00150 std::string gazeboTopicName, std::string rosTopicName,
00151 transport::NodePtr gz_node_handle);
00152
00153 std::vector<gazebo::transport::NodePtr> nodePtrs_;
00154 std::vector<gazebo::transport::SubscriberPtr> subscriberPtrs_;
00155
00156
00157
00159 transport::NodePtr gz_node_handle_;
00160
00162 ros::NodeHandle* ros_node_handle_;
00163
00165 physics::WorldPtr world_;
00166
00168 event::ConnectionPtr updateConnection_;
00169
00170
00171
00172
00173
00174 transport::SubscriberPtr gz_connect_gazebo_to_ros_topic_sub_;
00175 void GzConnectGazeboToRosTopicMsgCallback(
00176 GzConnectGazeboToRosTopicMsgPtr& gz_connect_gazebo_to_ros_topic_msg);
00177
00178
00179
00180
00181
00182 transport::SubscriberPtr gz_connect_ros_to_gazebo_topic_sub_;
00183
00187 void GzConnectRosToGazeboTopicMsgCallback(
00188 GzConnectRosToGazeboTopicMsgPtr& gz_connect_ros_to_gazebo_topic_msg);
00189
00195 template <typename T>
00196 transport::PublisherPtr FindOrMakeGazeboPublisher(std::string topic);
00197
00198
00199
00200
00201
00202 void ConvertHeaderGzToRos(
00203 const gz_std_msgs::Header& gz_header,
00204 std_msgs::Header_<std::allocator<void> >* ros_header);
00205
00206 void ConvertHeaderRosToGz(
00207 const std_msgs::Header_<std::allocator<void> >& ros_header,
00208 gz_std_msgs::Header* gz_header);
00209
00210
00211
00212
00213
00214
00215 void GzActuatorsMsgCallback(GzActuatorsMsgPtr& gz_actuators_msg,
00216 ros::Publisher ros_publisher);
00217 mav_msgs::Actuators ros_actuators_msg_;
00218
00219
00220 void GzFloat32MsgCallback(GzFloat32MsgPtr& gz_float_32_msg,
00221 ros::Publisher ros_publisher);
00222 std_msgs::Float32 ros_float_32_msg_;
00223
00224
00225 void GzFluidPressureMsgCallback(GzFluidPressureMsgPtr& gz_fluid_pressure_msg,
00226 ros::Publisher ros_publisher);
00227 sensor_msgs::FluidPressure ros_fluid_pressure_msg_;
00228
00229
00230 void GzImuMsgCallback(GzImuPtr& gz_imu_msg, ros::Publisher ros_publisher);
00231 sensor_msgs::Imu ros_imu_msg_;
00232
00233
00234 void GzJointStateMsgCallback(GzJointStateMsgPtr& gz_joint_state_msg,
00235 ros::Publisher ros_publisher);
00236 sensor_msgs::JointState ros_joint_state_msg_;
00237
00238
00239 void GzMagneticFieldMsgCallback(GzMagneticFieldMsgPtr& gz_magnetic_field_msg,
00240 ros::Publisher ros_publisher);
00241 sensor_msgs::MagneticField ros_magnetic_field_msg_;
00242
00243
00244 void GzNavSatFixCallback(GzNavSatFixPtr& gz_nav_sat_fix_msg,
00245 ros::Publisher ros_publisher);
00246 sensor_msgs::NavSatFix ros_nav_sat_fix_msg_;
00247
00248
00249 void GzOdometryMsgCallback(GzOdometryMsgPtr& gz_odometry_msg,
00250 ros::Publisher ros_publisher);
00251 nav_msgs::Odometry ros_odometry_msg_;
00252
00253
00254 void GzPoseMsgCallback(GzPoseMsgPtr& gz_pose_msg,
00255 ros::Publisher ros_publisher);
00256 geometry_msgs::Pose ros_pose_msg_;
00257
00258
00259 void GzPoseWithCovarianceStampedMsgCallback(
00260 GzPoseWithCovarianceStampedMsgPtr& gz_pose_with_covariance_stamped_msg,
00261 ros::Publisher ros_publisher);
00262 geometry_msgs::PoseWithCovarianceStamped
00263 ros_pose_with_covariance_stamped_msg_;
00264
00265
00266 void GzVector3dStampedMsgCallback(
00267 GzVector3dStampedMsgPtr& gz_vector_3d_stamped_msg,
00268 ros::Publisher ros_publisher);
00269 geometry_msgs::PointStamped ros_position_stamped_msg_;
00270
00271
00272 void GzTransformStampedMsgCallback(
00273 GzTransformStampedMsgPtr& gz_transform_stamped_msg,
00274 ros::Publisher ros_publisher);
00275 geometry_msgs::TransformStamped ros_transform_stamped_msg_;
00276
00277
00278 void GzTwistStampedMsgCallback(GzTwistStampedMsgPtr& gz_twist_stamped_msg,
00279 ros::Publisher ros_publisher);
00280 geometry_msgs::TwistStamped ros_twist_stamped_msg_;
00281
00282
00283 void GzWindSpeedMsgCallback(GzWindSpeedMsgPtr& gz_wind_speed_msg,
00284 ros::Publisher ros_publisher);
00285 rotors_comm::WindSpeed ros_wind_speed_msg_;
00286
00287
00288 void GzWrenchStampedMsgCallback(GzWrenchStampedMsgPtr& gz_wrench_stamped_msg,
00289 ros::Publisher ros_publisher);
00290 geometry_msgs::WrenchStamped ros_wrench_stamped_msg_;
00291
00292
00293
00294
00295
00296
00297 void RosActuatorsMsgCallback(
00298 const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr,
00299 gazebo::transport::PublisherPtr gz_publisher_ptr);
00300
00301
00302 void RosCommandMotorSpeedMsgCallback(
00303 const mav_msgs::ActuatorsConstPtr& ros_command_motor_speed_msg_ptr,
00304 gazebo::transport::PublisherPtr gz_publisher_ptr);
00305
00306
00307 void RosRollPitchYawrateThrustMsgCallback(
00308 const mav_msgs::RollPitchYawrateThrustConstPtr&
00309 ros_roll_pitch_yawrate_thrust_msg_ptr,
00310 gazebo::transport::PublisherPtr gz_publisher_ptr);
00311
00312
00313 void RosWindSpeedMsgCallback(
00314 const rotors_comm::WindSpeedConstPtr& ros_wind_speed_msg_ptr,
00315 gazebo::transport::PublisherPtr gz_publisher_ptr);
00316
00317
00318
00319
00320
00321 transport::SubscriberPtr gz_broadcast_transform_sub_;
00322
00327 void GzBroadcastTransformMsgCallback(
00328 GzTransformStampedWithFrameIdsMsgPtr& broadcast_transform_msg);
00329
00330 tf::Transform tf_;
00331 tf::TransformBroadcaster transform_broadcaster_;
00332 };
00333
00334 }
00335
00336 #endif // #ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H
rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43