gazebo_ros_interface_plugin.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H
18 #define ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H
19 
20 // SYSTEM INCLUDES
21 #include <random>
22 
23 #include <Eigen/Core>
24 #include <gazebo/common/Plugin.hh>
25 #include <gazebo/common/common.hh>
26 #include <gazebo/gazebo.hh>
27 #include <gazebo/physics/physics.hh>
28 #include "gazebo/msgs/msgs.hh"
29 
30 //=================== ROS =====================//
32 #include <ros/callback_queue.h>
33 #include <ros/ros.h>
35 
36 //============= GAZEBO MSG TYPES ==============//
37 #include "ConnectGazeboToRosTopic.pb.h"
38 #include "ConnectRosToGazeboTopic.pb.h"
39 
40 #include "Actuators.pb.h"
41 #include "CommandMotorSpeed.pb.h"
42 #include "Float32.pb.h"
43 #include "FluidPressure.pb.h"
44 #include "Imu.pb.h"
45 #include "JointState.pb.h"
46 #include "MagneticField.pb.h"
47 #include "NavSatFix.pb.h"
48 #include "Odometry.pb.h"
49 #include "PoseWithCovarianceStamped.pb.h"
50 #include "RollPitchYawrateThrust.pb.h"
51 #include "TransformStamped.pb.h"
52 #include "TransformStampedWithFrameIds.pb.h"
53 #include "TwistStamped.pb.h"
54 #include "Vector3dStamped.pb.h"
55 #include "WindSpeed.pb.h"
56 #include "WrenchStamped.pb.h"
57 
58 //=============== ROS MSG TYPES ===============//
59 #include <geometry_msgs/Point.h>
60 #include <geometry_msgs/PointStamped.h>
61 #include <geometry_msgs/Pose.h>
62 #include <geometry_msgs/PoseWithCovarianceStamped.h>
63 #include <geometry_msgs/TransformStamped.h>
64 #include <geometry_msgs/TwistStamped.h>
65 #include <geometry_msgs/WrenchStamped.h>
66 #include <mav_msgs/Actuators.h>
67 #include <mav_msgs/RollPitchYawrateThrust.h>
68 #include <nav_msgs/Odometry.h>
69 #include <rotors_comm/WindSpeed.h>
70 #include <sensor_msgs/FluidPressure.h>
71 #include <sensor_msgs/Imu.h>
72 #include <sensor_msgs/JointState.h>
73 #include <sensor_msgs/MagneticField.h>
74 #include <sensor_msgs/NavSatFix.h>
75 #include <std_msgs/Float32.h>
76 
78 
79 namespace gazebo {
80 
81 // typedef's to make life easier
90 typedef const boost::shared_ptr<
91  const gz_geometry_msgs::PoseWithCovarianceStamped>
95 typedef const boost::shared_ptr<
96  const gz_geometry_msgs::TransformStampedWithFrameIds>
117 
123 class GazeboRosInterfacePlugin : public WorldPlugin {
124  public:
127 
128  void InitializeParams();
129  void Publish();
130 
131  protected:
132  void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf);
133 
136  void OnUpdate(const common::UpdateInfo&);
137 
138  private:
145  template <typename GazeboMsgT, typename RosMsgT>
146  void ConnectHelper(void (GazeboRosInterfacePlugin::*fp)(
149  GazeboRosInterfacePlugin* ptr, std::string gazeboNamespace,
150  std::string gazeboTopicName, std::string rosTopicName,
151  transport::NodePtr gz_node_handle);
152 
153  std::vector<gazebo::transport::NodePtr> nodePtrs_;
154  std::vector<gazebo::transport::SubscriberPtr> subscriberPtrs_;
155 
156  // std::string namespace_;
157 
159  transport::NodePtr gz_node_handle_;
160 
163 
165  physics::WorldPtr world_;
166 
168  event::ConnectionPtr updateConnection_;
169 
170  // ============================================ //
171  // ====== CONNECT GAZEBO TO ROS MESSAGES ====== //
172  // ============================================ //
173 
174  transport::SubscriberPtr gz_connect_gazebo_to_ros_topic_sub_;
176  GzConnectGazeboToRosTopicMsgPtr& gz_connect_gazebo_to_ros_topic_msg);
177 
178  // ============================================ //
179  // ====== CONNECT ROS TO GAZEBO MESSAGES ====== //
180  // ============================================ //
181 
182  transport::SubscriberPtr gz_connect_ros_to_gazebo_topic_sub_;
183 
188  GzConnectRosToGazeboTopicMsgPtr& gz_connect_ros_to_gazebo_topic_msg);
189 
195  template <typename T>
196  transport::PublisherPtr FindOrMakeGazeboPublisher(std::string topic);
197 
198  // ============================================ //
199  // ===== HELPER METHODS FOR MSG CONVERSION ==== //
200  // ============================================ //
201 
203  const gz_std_msgs::Header& gz_header,
204  std_msgs::Header_<std::allocator<void> >* ros_header);
205 
207  const std_msgs::Header_<std::allocator<void> >& ros_header,
208  gz_std_msgs::Header* gz_header);
209 
210  // ============================================ //
211  // ===== GAZEBO->ROS CALLBACKS/CONVERTERS ===== //
212  // ============================================ //
213 
214  // ACTUATORS
215  void GzActuatorsMsgCallback(GzActuatorsMsgPtr& gz_actuators_msg,
216  ros::Publisher ros_publisher);
217  mav_msgs::Actuators ros_actuators_msg_;
218 
219  // FLOAT32
220  void GzFloat32MsgCallback(GzFloat32MsgPtr& gz_float_32_msg,
221  ros::Publisher ros_publisher);
222  std_msgs::Float32 ros_float_32_msg_;
223 
224  // FLUID PRESSURE
225  void GzFluidPressureMsgCallback(GzFluidPressureMsgPtr& gz_fluid_pressure_msg,
226  ros::Publisher ros_publisher);
227  sensor_msgs::FluidPressure ros_fluid_pressure_msg_;
228 
229  // IMU
230  void GzImuMsgCallback(GzImuPtr& gz_imu_msg, ros::Publisher ros_publisher);
231  sensor_msgs::Imu ros_imu_msg_;
232 
233  // JOINT STATE
234  void GzJointStateMsgCallback(GzJointStateMsgPtr& gz_joint_state_msg,
235  ros::Publisher ros_publisher);
236  sensor_msgs::JointState ros_joint_state_msg_;
237 
238  // MAGNETIC FIELD
239  void GzMagneticFieldMsgCallback(GzMagneticFieldMsgPtr& gz_magnetic_field_msg,
240  ros::Publisher ros_publisher);
241  sensor_msgs::MagneticField ros_magnetic_field_msg_;
242 
243  // NAT SAT FIX (GPS)
244  void GzNavSatFixCallback(GzNavSatFixPtr& gz_nav_sat_fix_msg,
245  ros::Publisher ros_publisher);
246  sensor_msgs::NavSatFix ros_nav_sat_fix_msg_;
247 
248  // ODOMETRY
249  void GzOdometryMsgCallback(GzOdometryMsgPtr& gz_odometry_msg,
250  ros::Publisher ros_publisher);
251  nav_msgs::Odometry ros_odometry_msg_;
252 
253  // POSE
254  void GzPoseMsgCallback(GzPoseMsgPtr& gz_pose_msg,
255  ros::Publisher ros_publisher);
256  geometry_msgs::Pose ros_pose_msg_;
257 
258  // POSE WITH COVARIANCE STAMPED
260  GzPoseWithCovarianceStampedMsgPtr& gz_pose_with_covariance_stamped_msg,
261  ros::Publisher ros_publisher);
262  geometry_msgs::PoseWithCovarianceStamped
264 
265  // POSITION STAMPED
267  GzVector3dStampedMsgPtr& gz_vector_3d_stamped_msg,
268  ros::Publisher ros_publisher);
269  geometry_msgs::PointStamped ros_position_stamped_msg_;
270 
271  // TRANSFORM STAMPED
273  GzTransformStampedMsgPtr& gz_transform_stamped_msg,
274  ros::Publisher ros_publisher);
275  geometry_msgs::TransformStamped ros_transform_stamped_msg_;
276 
277  // TWIST STAMPED
278  void GzTwistStampedMsgCallback(GzTwistStampedMsgPtr& gz_twist_stamped_msg,
279  ros::Publisher ros_publisher);
280  geometry_msgs::TwistStamped ros_twist_stamped_msg_;
281 
282  // WIND SPEED
283  void GzWindSpeedMsgCallback(GzWindSpeedMsgPtr& gz_wind_speed_msg,
284  ros::Publisher ros_publisher);
285  rotors_comm::WindSpeed ros_wind_speed_msg_;
286 
287  // WRENCH STAMPED
288  void GzWrenchStampedMsgCallback(GzWrenchStampedMsgPtr& gz_wrench_stamped_msg,
289  ros::Publisher ros_publisher);
290  geometry_msgs::WrenchStamped ros_wrench_stamped_msg_;
291 
292  // ============================================ //
293  // ===== ROS->GAZEBO CALLBACKS/CONVERTERS ===== //
294  // ============================================ //
295 
296  // ACTUATORS (change name??? motor control? motor speed?)
298  const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr,
299  gazebo::transport::PublisherPtr gz_publisher_ptr);
300 
301  // COMMAND MOTOR SPEED (this is the same as ACTUATORS!, merge???)
303  const mav_msgs::ActuatorsConstPtr& ros_command_motor_speed_msg_ptr,
304  gazebo::transport::PublisherPtr gz_publisher_ptr);
305 
306  // ROLL PITCH YAWRATE THRUST
308  const mav_msgs::RollPitchYawrateThrustConstPtr&
309  ros_roll_pitch_yawrate_thrust_msg_ptr,
310  gazebo::transport::PublisherPtr gz_publisher_ptr);
311 
312  // WIND SPEED
314  const rotors_comm::WindSpeedConstPtr& ros_wind_speed_msg_ptr,
315  gazebo::transport::PublisherPtr gz_publisher_ptr);
316 
317  // ============================================ //
318  // ====== TRANSFORM BROADCASTER RELATED ======= //
319  // ============================================ //
320 
321  transport::SubscriberPtr gz_broadcast_transform_sub_;
322 
328  GzTransformStampedWithFrameIdsMsgPtr& broadcast_transform_msg);
329 
332 };
333 
334 } // namespace gazebo
335 
336 #endif // #ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H
const boost::shared_ptr< const gz_geometry_msgs::TransformStamped > GzTransformStampedMsgPtr
void ConvertHeaderGzToRos(const gz_std_msgs::Header &gz_header, std_msgs::Header_< std::allocator< void > > *ros_header)
const boost::shared_ptr< const gz_sensor_msgs::NavSatFix > GzNavSatFixPtr
const boost::shared_ptr< const gz_mav_msgs::WindSpeed > GzWindSpeedMsgPtr
const boost::shared_ptr< const gz_geometry_msgs::Odometry > GzOdometryMsgPtr
void GzFloat32MsgCallback(GzFloat32MsgPtr &gz_float_32_msg, ros::Publisher ros_publisher)
const boost::shared_ptr< const gazebo::msgs::Pose > GzPoseMsgPtr
void GzWindSpeedMsgCallback(GzWindSpeedMsgPtr &gz_wind_speed_msg, ros::Publisher ros_publisher)
void GzTwistStampedMsgCallback(GzTwistStampedMsgPtr &gz_twist_stamped_msg, ros::Publisher ros_publisher)
void GzVector3dStampedMsgCallback(GzVector3dStampedMsgPtr &gz_vector_3d_stamped_msg, ros::Publisher ros_publisher)
void GzOdometryMsgCallback(GzOdometryMsgPtr &gz_odometry_msg, ros::Publisher ros_publisher)
const boost::shared_ptr< const gz_geometry_msgs::Vector3dStamped > GzVector3dStampedMsgPtr
const boost::shared_ptr< const gz_geometry_msgs::TwistStamped > GzTwistStampedMsgPtr
ros::NodeHandle * ros_node_handle_
Handle for the ROS node.
const boost::shared_ptr< const gz_sensor_msgs::FluidPressure > GzFluidPressureMsgPtr
transport::PublisherPtr FindOrMakeGazeboPublisher(std::string topic)
Looks if a publisher on the provided topic already exists, and returns it. If no publisher exists...
const boost::shared_ptr< const gz_geometry_msgs::PoseWithCovarianceStamped > GzPoseWithCovarianceStampedMsgPtr
void GzNavSatFixCallback(GzNavSatFixPtr &gz_nav_sat_fix_msg, ros::Publisher ros_publisher)
void GzWrenchStampedMsgCallback(GzWrenchStampedMsgPtr &gz_wrench_stamped_msg, ros::Publisher ros_publisher)
const boost::shared_ptr< const gz_sensor_msgs::Imu > GzImuPtr
void GzConnectGazeboToRosTopicMsgCallback(GzConnectGazeboToRosTopicMsgPtr &gz_connect_gazebo_to_ros_topic_msg)
void GzTransformStampedMsgCallback(GzTransformStampedMsgPtr &gz_transform_stamped_msg, ros::Publisher ros_publisher)
const boost::shared_ptr< const gz_geometry_msgs::WrenchStamped > GzWrenchStampedMsgPtr
void GzActuatorsMsgCallback(GzActuatorsMsgPtr &gz_actuators_msg, ros::Publisher ros_publisher)
const boost::shared_ptr< const gz_sensor_msgs::Actuators > GzActuatorsMsgPtr
void RosRollPitchYawrateThrustMsgCallback(const mav_msgs::RollPitchYawrateThrustConstPtr &ros_roll_pitch_yawrate_thrust_msg_ptr, gazebo::transport::PublisherPtr gz_publisher_ptr)
void GzConnectRosToGazeboTopicMsgCallback(GzConnectRosToGazeboTopicMsgPtr &gz_connect_ros_to_gazebo_topic_msg)
Subscribes to the provided ROS topic and publishes on the provided Gazebo topic (all info contained w...
geometry_msgs::WrenchStamped ros_wrench_stamped_msg_
geometry_msgs::PoseWithCovarianceStamped ros_pose_with_covariance_stamped_msg_
void RosWindSpeedMsgCallback(const rotors_comm::WindSpeedConstPtr &ros_wind_speed_msg_ptr, gazebo::transport::PublisherPtr gz_publisher_ptr)
void ConnectHelper(void(GazeboRosInterfacePlugin::*fp)(const boost::shared_ptr< GazeboMsgT const > &, ros::Publisher), GazeboRosInterfacePlugin *ptr, std::string gazeboNamespace, std::string gazeboTopicName, std::string rosTopicName, transport::NodePtr gz_node_handle)
Provides a way for GzConnectGazeboToRosTopicMsgCallback() to connect a Gazebo subscriber to a ROS pub...
void GzImuMsgCallback(GzImuPtr &gz_imu_msg, ros::Publisher ros_publisher)
const boost::shared_ptr< const gz_std_msgs::Float32 > GzFloat32MsgPtr
void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
std::vector< gazebo::transport::NodePtr > nodePtrs_
void GzMagneticFieldMsgCallback(GzMagneticFieldMsgPtr &gz_magnetic_field_msg, ros::Publisher ros_publisher)
geometry_msgs::PointStamped ros_position_stamped_msg_
void GzPoseWithCovarianceStampedMsgCallback(GzPoseWithCovarianceStampedMsgPtr &gz_pose_with_covariance_stamped_msg, ros::Publisher ros_publisher)
geometry_msgs::TwistStamped ros_twist_stamped_msg_
sensor_msgs::MagneticField ros_magnetic_field_msg_
std::vector< gazebo::transport::SubscriberPtr > subscriberPtrs_
const boost::shared_ptr< const gz_std_msgs::ConnectGazeboToRosTopic > GzConnectGazeboToRosTopicMsgPtr
const boost::shared_ptr< const gz_sensor_msgs::JointState > GzJointStateMsgPtr
geometry_msgs::TransformStamped ros_transform_stamped_msg_
sensor_msgs::FluidPressure ros_fluid_pressure_msg_
const boost::shared_ptr< const gz_mav_msgs::RollPitchYawrateThrust > GzRollPitchYawrateThrustPtr
physics::WorldPtr world_
Pointer to the world.
void GzPoseMsgCallback(GzPoseMsgPtr &gz_pose_msg, ros::Publisher ros_publisher)
const boost::shared_ptr< const gz_sensor_msgs::MagneticField > GzMagneticFieldMsgPtr
ROS interface plugin for Gazebo.
void GzFluidPressureMsgCallback(GzFluidPressureMsgPtr &gz_fluid_pressure_msg, ros::Publisher ros_publisher)
transport::SubscriberPtr gz_connect_ros_to_gazebo_topic_sub_
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
transport::SubscriberPtr gz_broadcast_transform_sub_
void ConvertHeaderRosToGz(const std_msgs::Header_< std::allocator< void > > &ros_header, gz_std_msgs::Header *gz_header)
void OnUpdate(const common::UpdateInfo &)
This gets called by the world update start event.
void GzJointStateMsgCallback(GzJointStateMsgPtr &gz_joint_state_msg, ros::Publisher ros_publisher)
transport::SubscriberPtr gz_connect_gazebo_to_ros_topic_sub_
const boost::shared_ptr< const gz_std_msgs::ConnectRosToGazeboTopic > GzConnectRosToGazeboTopicMsgPtr
transport::NodePtr gz_node_handle_
Handle for the Gazebo node.
void RosActuatorsMsgCallback(const mav_msgs::ActuatorsConstPtr &ros_actuators_msg_ptr, gazebo::transport::PublisherPtr gz_publisher_ptr)
const boost::shared_ptr< const gz_geometry_msgs::TransformStampedWithFrameIds > GzTransformStampedWithFrameIdsMsgPtr
void RosCommandMotorSpeedMsgCallback(const mav_msgs::ActuatorsConstPtr &ros_command_motor_speed_msg_ptr, gazebo::transport::PublisherPtr gz_publisher_ptr)
void GzBroadcastTransformMsgCallback(GzTransformStampedWithFrameIdsMsgPtr &broadcast_transform_msg)
This is a special-case callback which listens for Gazebo "Transform" messages. Upon receiving one it ...


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Mon Feb 28 2022 23:39:04