17 #ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H 18 #define ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H 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" 37 #include "ConnectGazeboToRosTopic.pb.h" 38 #include "ConnectRosToGazeboTopic.pb.h" 40 #include "Actuators.pb.h" 41 #include "CommandMotorSpeed.pb.h" 42 #include "Float32.pb.h" 43 #include "FluidPressure.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" 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> 91 const gz_geometry_msgs::PoseWithCovarianceStamped>
96 const gz_geometry_msgs::TransformStampedWithFrameIds>
132 void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf);
136 void OnUpdate(
const common::UpdateInfo&);
145 template <
typename GazeboMsgT,
typename RosMsgT>
150 std::string gazeboTopicName, std::string rosTopicName,
151 transport::NodePtr gz_node_handle);
195 template <
typename T>
203 const gz_std_msgs::Header& gz_header,
208 gz_std_msgs::Header* gz_header);
262 geometry_msgs::PoseWithCovarianceStamped
298 const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr,
299 gazebo::transport::PublisherPtr gz_publisher_ptr);
303 const mav_msgs::ActuatorsConstPtr& ros_command_motor_speed_msg_ptr,
304 gazebo::transport::PublisherPtr gz_publisher_ptr);
308 const mav_msgs::RollPitchYawrateThrustConstPtr&
309 ros_roll_pitch_yawrate_thrust_msg_ptr,
310 gazebo::transport::PublisherPtr gz_publisher_ptr);
314 const rotors_comm::WindSpeedConstPtr& ros_wind_speed_msg_ptr,
315 gazebo::transport::PublisherPtr gz_publisher_ptr);
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)
rotors_comm::WindSpeed ros_wind_speed_msg_
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.
mav_msgs::Actuators ros_actuators_msg_
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
tf::TransformBroadcaster transform_broadcaster_
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_
geometry_msgs::Pose ros_pose_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)
nav_msgs::Odometry ros_odometry_msg_
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_
std_msgs::Float32 ros_float_32_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
sensor_msgs::JointState ros_joint_state_msg_
geometry_msgs::TransformStamped ros_transform_stamped_msg_
sensor_msgs::FluidPressure ros_fluid_pressure_msg_
const boost::shared_ptr< const gz_mav_msgs::RollPitchYawrateThrust > GzRollPitchYawrateThrustPtr
sensor_msgs::NavSatFix ros_nav_sat_fix_msg_
physics::WorldPtr world_
Pointer to the world.
sensor_msgs::Imu ros_imu_msg_
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)
~GazeboRosInterfacePlugin()
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.
GazeboRosInterfacePlugin()
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 ...