rotors_gazebo_plugins: gazebo_ros_interface

Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H
00018 #define ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H
00019 
00020 // SYSTEM INCLUDES
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 //=================== ROS =====================//
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 //============= GAZEBO MSG TYPES ==============//
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 //=============== ROS MSG TYPES ===============//
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 // typedef's to make life easier
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   // std::string namespace_;
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   // ====== CONNECT GAZEBO TO ROS MESSAGES ====== //
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   // ====== CONNECT ROS TO GAZEBO MESSAGES ====== //
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   // ===== HELPER METHODS FOR MSG CONVERSION ==== //
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   // ===== GAZEBO->ROS CALLBACKS/CONVERTERS ===== //
00212   // ============================================ //
00213 
00214   // ACTUATORS
00215   void GzActuatorsMsgCallback(GzActuatorsMsgPtr& gz_actuators_msg,
00216                               ros::Publisher ros_publisher);
00217   mav_msgs::Actuators ros_actuators_msg_;
00218 
00219   // FLOAT32
00220   void GzFloat32MsgCallback(GzFloat32MsgPtr& gz_float_32_msg,
00221                             ros::Publisher ros_publisher);
00222   std_msgs::Float32 ros_float_32_msg_;
00223 
00224   // FLUID PRESSURE
00225   void GzFluidPressureMsgCallback(GzFluidPressureMsgPtr& gz_fluid_pressure_msg,
00226                                   ros::Publisher ros_publisher);
00227   sensor_msgs::FluidPressure ros_fluid_pressure_msg_;
00228 
00229   // IMU
00230   void GzImuMsgCallback(GzImuPtr& gz_imu_msg, ros::Publisher ros_publisher);
00231   sensor_msgs::Imu ros_imu_msg_;
00232 
00233   // JOINT STATE
00234   void GzJointStateMsgCallback(GzJointStateMsgPtr& gz_joint_state_msg,
00235                                ros::Publisher ros_publisher);
00236   sensor_msgs::JointState ros_joint_state_msg_;
00237 
00238   // MAGNETIC FIELD
00239   void GzMagneticFieldMsgCallback(GzMagneticFieldMsgPtr& gz_magnetic_field_msg,
00240                                   ros::Publisher ros_publisher);
00241   sensor_msgs::MagneticField ros_magnetic_field_msg_;
00242 
00243   // NAT SAT FIX (GPS)
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   // ODOMETRY
00249   void GzOdometryMsgCallback(GzOdometryMsgPtr& gz_odometry_msg,
00250                              ros::Publisher ros_publisher);
00251   nav_msgs::Odometry ros_odometry_msg_;
00252 
00253   // POSE
00254   void GzPoseMsgCallback(GzPoseMsgPtr& gz_pose_msg,
00255                          ros::Publisher ros_publisher);
00256   geometry_msgs::Pose ros_pose_msg_;
00257 
00258   // POSE WITH COVARIANCE STAMPED
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   // POSITION STAMPED
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   // TRANSFORM STAMPED
00272   void GzTransformStampedMsgCallback(
00273       GzTransformStampedMsgPtr& gz_transform_stamped_msg,
00274       ros::Publisher ros_publisher);
00275   geometry_msgs::TransformStamped ros_transform_stamped_msg_;
00276 
00277   // TWIST STAMPED
00278   void GzTwistStampedMsgCallback(GzTwistStampedMsgPtr& gz_twist_stamped_msg,
00279                                  ros::Publisher ros_publisher);
00280   geometry_msgs::TwistStamped ros_twist_stamped_msg_;
00281 
00282   // WIND SPEED
00283   void GzWindSpeedMsgCallback(GzWindSpeedMsgPtr& gz_wind_speed_msg,
00284                               ros::Publisher ros_publisher);
00285   rotors_comm::WindSpeed ros_wind_speed_msg_;
00286 
00287   // WRENCH STAMPED
00288   void GzWrenchStampedMsgCallback(GzWrenchStampedMsgPtr& gz_wrench_stamped_msg,
00289                                   ros::Publisher ros_publisher);
00290   geometry_msgs::WrenchStamped ros_wrench_stamped_msg_;
00291 
00292   // ============================================ //
00293   // ===== ROS->GAZEBO CALLBACKS/CONVERTERS ===== //
00294   // ============================================ //
00295 
00296   // ACTUATORS (change name??? motor control? motor speed?)
00297   void RosActuatorsMsgCallback(
00298       const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr,
00299       gazebo::transport::PublisherPtr gz_publisher_ptr);
00300 
00301   // COMMAND MOTOR SPEED (this is the same as ACTUATORS!, merge???)
00302   void RosCommandMotorSpeedMsgCallback(
00303       const mav_msgs::ActuatorsConstPtr& ros_command_motor_speed_msg_ptr,
00304       gazebo::transport::PublisherPtr gz_publisher_ptr);
00305 
00306   // ROLL PITCH YAWRATE THRUST
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   // WIND SPEED
00313   void RosWindSpeedMsgCallback(
00314       const rotors_comm::WindSpeedConstPtr& ros_wind_speed_msg_ptr,
00315       gazebo::transport::PublisherPtr gz_publisher_ptr);
00316 
00317   // ============================================ //
00318   // ====== TRANSFORM BROADCASTER RELATED ======= //
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 }  // namespace gazebo
00335 
00336 #endif  // #ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H