21 #include "ConnectGazeboToRosTopic.pb.h" 28 pubs_and_subs_created_(false) {
36 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
39 gzdbg <<
"_model = " << _model->GetName() << std::endl;
50 if (_sdf->HasElement(
"robotNamespace"))
51 namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
53 gzerr <<
"[gazebo_pressure_plugin] Please specify a robotNamespace.\n";
56 node_handle_ = transport::NodePtr(
new transport::Node());
61 std::string link_name;
62 if (_sdf->HasElement(
"linkName"))
63 link_name = _sdf->GetElement(
"linkName")->Get<std::string>();
65 gzerr <<
"[gazebo_pressure_plugin] Please specify a linkName.\n";
69 gzthrow(
"[gazebo_pressure_plugin] Couldn't find specified link \"" << link_name <<
"\".");
77 CHECK(pressure_var_ >= 0.0);
98 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
106 common::Time current_time =
world_->SimTime();
109 double height_geometric_m =
ref_alt_ +
model_->WorldPose().Pos().Z();
116 double temperature_at_altitude_kelvin =
120 double pressure_at_altitude_pascal =
142 gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
143 node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
153 gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
154 connect_gazebo_to_ros_topic_msg.set_gazebo_topic(
"~/" +
namespace_ +
"/" +
156 connect_gazebo_to_ros_topic_msg.set_ros_topic(
namespace_ +
"/" +
158 connect_gazebo_to_ros_topic_msg.set_msgtype(
159 gz_std_msgs::ConnectGazeboToRosTopic::FLUID_PRESSURE);
160 connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
static constexpr double kEarthRadiusMeters
std::normal_distribution NormalDistribution
event::ConnectionPtr updateConnection_
Pointer to the update event connection.
physics::LinkPtr link_
Pointer to the link.
NormalDistribution pressure_n_[1]
Normal distribution for pressure noise.
double pressure_var_
Pressure measurement variance (Pa^2).
physics::ModelPtr model_
Pointer to the model.
GazeboPressurePlugin()
Constructor.
physics::WorldPtr world_
Pointer to the world.
std::string frame_id_
Frame ID for pressure messages.
void OnUpdate(const common::UpdateInfo &)
This gets called by the world update start event.
gz_sensor_msgs::FluidPressure pressure_message_
Fluid pressure message.
gazebo::transport::PublisherPtr pressure_pub_
Pressure message publisher.
gazebo::transport::NodePtr node_handle_
Handle for the Gazebo node.
bool pubs_and_subs_created_
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from...
static constexpr double kPressureOneAtmospherePascals
static const std::string kDefaultPressurePubTopic
static constexpr double kDefaultPressureVar
double ref_alt_
Reference altitude (meters).
std::mt19937 random_generator_
static const bool kPrintOnPluginLoad
static constexpr double kSeaLevelTempKelvin
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
static constexpr double kTempLapseKelvinPerMeter
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
Called when the plugin is first created, and after the world has been loaded. This function should no...
static constexpr double kDefaultRefAlt
virtual ~GazeboPressurePlugin()
Destructor.
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
static constexpr double kAirConstantDimensionless
void CreatePubsAndSubs()
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required...
static const bool kPrintOnUpdates
GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D)
static const std::string kConnectGazeboToRosSubtopic
std::string namespace_
Transport namespace.
std::string pressure_topic_
Topic name for pressure messages.