diff --git a/vrx_gz/CMakeLists.txt b/vrx_gz/CMakeLists.txt index 9cc92d09e..78fcbaf7b 100644 --- a/vrx_gz/CMakeLists.txt +++ b/vrx_gz/CMakeLists.txt @@ -20,6 +20,8 @@ find_package(gz-plugin2 REQUIRED COMPONENTS loader register) set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) find_package(gz-rendering7 REQUIRED) set(GZ_RENDERING_VER ${gz-rendering7_VERSION_MAJOR}) +find_package(gz-sensors7 REQUIRED) +set(GZ_SENSORS_VER ${gz-sensors7_VERSION_MAJOR}) find_package(gz-utils2 REQUIRED) set(GZ_UTILS_VER ${gz-utils2_VERSION_MAJOR}) find_package(sdformat13 REQUIRED) @@ -96,6 +98,7 @@ install( # Plugins list(APPEND VRX_GZ_PLUGINS + AcousticPingerPlugin PerceptionScoringPlugin SimpleHydrodynamics StationkeepingScoringPlugin @@ -109,6 +112,7 @@ foreach(PLUGIN ${VRX_GZ_PLUGINS}) gz-sim${GZ_SIM_VER}::core gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} + gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER} ScoringPlugin Waves diff --git a/vrx_gz/models/wam-v/model.sdf.erb b/vrx_gz/models/wam-v/model.sdf.erb index b9034cc23..92ec1d445 100644 --- a/vrx_gz/models/wam-v/model.sdf.erb +++ b/vrx_gz/models/wam-v/model.sdf.erb @@ -250,5 +250,37 @@ end 1 + + + -528 191 -2 + <%= $model_name%>/pingers/pinger/range_bearing + <%= $model_name%>/pingers/pinger/set_pinger_position + + <%= $model_name%>/pinger + + + gaussian + 0.0 + 3.0 + + + + + gaussian + 0.0 + 0.01 + + + + + gaussian + 0.0 + 0.01 + + + + diff --git a/vrx_gz/src/AcousticPingerPlugin.cc b/vrx_gz/src/AcousticPingerPlugin.cc new file mode 100644 index 000000000..f581e61ce --- /dev/null +++ b/vrx_gz/src/AcousticPingerPlugin.cc @@ -0,0 +1,315 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "AcousticPingerPlugin.hh" + +using namespace gz; +using namespace vrx; + +/// \brief Private AcousticPingerPlugin data class. +class AcousticPingerPlugin::Implementation +{ + /// \brief Parse all SDF parameters. + /// \param[in] _ecm The Entity Component Manager. + /// \return True when all params were successfully parsed or false otherwise. + public: bool ParseSDFParameters(sim::EntityComponentManager &_ecm); + + /// \brief Callback function called when receiving a new pinger position + /// via the pinger subscription callback. + /// \param[in] _pos New pinger position. + public: void PingerPositionCallback(const msgs::Vector3d &_pos); + + /// \brief Transport node. + public: transport::Node node; + + /// \brief Publisher used to send sensor messages generated by the plugin. + public: transport::Node::Publisher rangeBearingPub; + + /// \brief Mutex to protect the position vector. + public: std::mutex mutex; + + /// \brief Entity of the model attached to the plugin. + public: sim::Entity entity = sim::kNullEntity; + + /// \brief Vector storing the position of the pinger. + public: math::Vector3d position = math::Vector3d::Zero; + + /// \brief String holding the frame id of the sensor. + public: std::string frameId = "pinger"; + + /// \brief The topic used to read the range bearing measurements. + public: std::string topic = "/pinger/range_bearing"; + + /// \brief The topic to set the position of the pinger. + public: std::string setPositionTopicName = "/pinger/set_pinger_position"; + + /// \brief Sensor update rate. + public: float updateRate = 1.0f; + + /// \brief The next message to be published. + public: msgs::Param msg; + + /// \brief SDF pointer. + public: sdf::ElementPtr sdf; + + /// \brief Variable used to track time of last update. This is used to + /// produce data at the correct rate. + public: std::chrono::duration lastUpdateTime{0}; + + // From Brian Bingham's rangebearing_gazebo_plugin. + /// \brief rangeNoise - Gazebo noise object for range. + public: sensors::NoisePtr rangeNoise = nullptr; + + /// \brief Gazebo noise object for bearing angle. + public: sensors::NoisePtr bearingNoise = nullptr; + + /// \brief Gazebo noise object for elevation angle. + public: sensors::NoisePtr elevationNoise = nullptr; +}; + +////////////////////////////////////////////////// +bool AcousticPingerPlugin::Implementation::ParseSDFParameters( + sim::EntityComponentManager &_ecm) +{ + // Set the frame_id. Defaults to "pinger". + if (this->sdf->HasElement("frame_id")) + this->frameId = this->sdf->Get("frame_id"); + + // Load topic from SDF if available. + if (this->sdf->HasElement("topic")) + this->topic = this->sdf->Get("topic"); + + // Set the topic to be used to publish the sensor message. + if (this->sdf->HasElement("set_position_topic")) + { + this->setPositionTopicName = + this->sdf->Get("set_position_topic"); + } + + // Initialize pinger position. Defaults to origin. + if (this->sdf->HasElement("position")) + this->position = this->sdf->Get("position"); + + // Initialise update rate. Default to 1 reading per second. + if (this->sdf->HasElement("update_rate")) + this->updateRate = this->sdf->Get("update_rate"); + + // From Brian Bingham's rangebearing_gazebo_plugin. + // Noise setup and parse SDF. + if (this->sdf->HasElement("range_noise")) + { + sdf::ElementPtr rangeNoiseElem = this->sdf->GetElement("range_noise"); + // Note - it is hardcoded into the NoiseFactory.cc that the SDF + // element be "noise". + if (rangeNoiseElem->HasElement("noise")) + { + this->rangeNoise = sensors::NoiseFactory::NewNoiseModel( + rangeNoiseElem->GetElement("noise")); + } + else + { + gzerr << " must contain a noise tag" << std::endl; + } + } + + // Load the noise model from the SDF file. + if (this->sdf->HasElement("bearing_noise")) + { + sdf::ElementPtr bearingNoiseElem = this->sdf->GetElement("bearing_noise"); + // Note - it is hardcoded into the NoiseFactory.cc that the SDF + // element be "noise". + if (bearingNoiseElem->HasElement("noise")) + { + this->bearingNoise = sensors::NoiseFactory::NewNoiseModel( + bearingNoiseElem->GetElement("noise")); + } + else + { + gzerr << " must contain a noise tag" << std::endl; + } + } + + if (this->sdf->HasElement("elevation_noise")) + { + sdf::ElementPtr elevationNoiseElem = + this->sdf->GetElement("elevation_noise"); + // Note - it is hardcoded into the NoiseFactory.cc that the SDF + // element be "noise". + if (elevationNoiseElem->HasElement("noise")) + { + this->elevationNoise = sensors::NoiseFactory::NewNoiseModel( + elevationNoiseElem->GetElement("noise")); + } + else + { + gzerr << " must contain a noise tag" << std::endl; + } + } + + return true; +} + +////////////////////////////////////////////////// +void AcousticPingerPlugin::Implementation::PingerPositionCallback( + const msgs::Vector3d &_pos) +{ + std::lock_guard lock(this->mutex); + this->position = math::Vector3d(_pos.x(), _pos.y(), _pos.z()); +} + +////////////////////////////////////////////////// +AcousticPingerPlugin::AcousticPingerPlugin() + : dataPtr(utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +void AcousticPingerPlugin::Configure(const sim::Entity &_entity, + const std::shared_ptr &_sdf, + sim::EntityComponentManager &_ecm, sim::EventManager &_eventMgr) +{ + this->dataPtr->sdf = _sdf->Clone(); + if (!this->dataPtr->ParseSDFParameters(_ecm)) + { + gzerr << "AcousticPingerPlugin disabled" << std::endl; + return; + } + + // Setup the publisher. + this->dataPtr->rangeBearingPub = + this->dataPtr->node.Advertise(this->dataPtr->topic); + + // Subscriber to update the pinger pose. + this->dataPtr->node.Subscribe(this->dataPtr->setPositionTopicName, + &AcousticPingerPlugin::Implementation::PingerPositionCallback, + this->dataPtr.get()); + + this->dataPtr->entity = _entity; + + // Prepopulate the msg. + auto *param = this->dataPtr->msg.mutable_params(); + + msgs::Any rangeValue; + rangeValue.set_type(msgs::Any_ValueType::Any_ValueType_DOUBLE); + rangeValue.set_double_value(0.0); + (*param)["range"] = rangeValue; + + msgs::Any bearingValue; + bearingValue.set_type(msgs::Any_ValueType::Any_ValueType_DOUBLE); + bearingValue.set_double_value(0.0); + (*param)["bearing"] = bearingValue; + + msgs::Any elevationValue; + elevationValue.set_type(msgs::Any_ValueType::Any_ValueType_DOUBLE); + elevationValue.set_double_value(0.0); + (*param)["elevation"] = elevationValue; +} + +////////////////////////////////////////////////// +void AcousticPingerPlugin::PreUpdate(const sim::UpdateInfo &_info, + sim::EntityComponentManager &_ecm) +{ + if (_info.paused) + return; + + if ((_info.simTime - this->dataPtr->lastUpdateTime) < + (std::chrono::duration(1.0) / this->dataPtr->updateRate)) + { + return; + } + + this->dataPtr->lastUpdateTime = _info.simTime; + + auto modelPose = + _ecm.Component(this->dataPtr->entity)->Data(); + + math::Vector3d direction; + { + std::lock_guard lock(this->dataPtr->mutex); + + // Direction vector to the pinger from the USV. + direction = this->dataPtr->position - modelPose.Pos(); + } + + // Sensor reading is in the sensor frame. Rotate the direction vector into + // the frame of reference of the sensor. + math::Vector3d directionSensorFrame = + modelPose.Rot().RotateVectorReverse(direction); + + // Generate a 2D vector for elevation calculation. + math::Vector3d directionSensorFrame2d = + math::Vector3d(directionSensorFrame.X(), directionSensorFrame.Y(), 0); + + // Bearing is calculated by finding the world frame direction vector + // and transforming into the pose of the sensor. Bearing is calculated + // using the atan2 function of the x and y components of the transformed + // vector. The elevation is calculated from the length of the 2D only + // and the z component of the sensor frame vector. + double bearing = atan2(directionSensorFrame.Y(), directionSensorFrame.X()); + double range = directionSensorFrame.Length(); + double elevation = + atan2(directionSensorFrame.Z(), directionSensorFrame2d.Length()); + + // Apply noise to each measurement. + // From Brian Binghams rangebearing_gazebo_plugin. + if (this->dataPtr->rangeNoise) + range = this->dataPtr->rangeNoise->Apply(range); + if (this->dataPtr->bearingNoise) + bearing = this->dataPtr->bearingNoise->Apply(bearing); + if (this->dataPtr->elevationNoise) + elevation = this->dataPtr->elevationNoise->Apply(elevation); + + // Update message + // - stamp + auto stamp = sim::convert(_info.simTime); + this->dataPtr->msg.mutable_header()->mutable_stamp()->CopyFrom(stamp); + + // - frame_id + auto frame = this->dataPtr->msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->dataPtr->frameId); + + // - range, bearing and elevation + auto *param = this->dataPtr->msg.mutable_params(); + (*param)["range"].set_double_value(range); + (*param)["bearing"].set_double_value(bearing); + (*param)["elevation"].set_double_value(elevation); + + // Publish a new measurement. + this->dataPtr->rangeBearingPub.Publish(this->dataPtr->msg); +} + +GZ_ADD_PLUGIN(AcousticPingerPlugin, + sim::System, + AcousticPingerPlugin::ISystemConfigure, + AcousticPingerPlugin::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(vrx::AcousticPingerPlugin, + "vrx::AcousticPingerPlugin") diff --git a/vrx_gz/src/AcousticPingerPlugin.hh b/vrx_gz/src/AcousticPingerPlugin.hh new file mode 100644 index 000000000..8bf19999e --- /dev/null +++ b/vrx_gz/src/AcousticPingerPlugin.hh @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef VRX_ACOUSTIC_PINGER_PLUGIN_HH_ +#define VRX_ACOUSTIC_PINGER_PLUGIN_HH_ + +#include +#include +#include +#include +#include + +namespace vrx +{ + /// \brief Implements a range and bearing pinger system. This assumes that + /// the ppinger localisation has a mechanism for estimating the range and + /// bearing of the pinger. Pinger estimates are published using messages. + /// + /// Bearings are reported in the vehicle frame and coordinate system: + /// the x-axis is towards the vehicle’s nose, the y-axis is towards the port + /// side, and the z-axis points upwards. Following the right-hand rule, + /// bearing angles are measured counter-clockwise beginning from the x-axis. + /// See https://github.com/osrf/vrx/wiki/frame_conventions#acoustic-pinger . + /// + /// Accepts the following SDF parameters: + /// Tf frame of the sensor message. Used as part of the sensor + /// message publication. + /// Name of the topic that the sensor message will be published on. + /// Defaults to "/pinger/range_bearing". + /// Name of the topic that is used to set the position + /// of the simulated pinger sensor. + /// Defaults to "/pinger/set_pinger_position". + /// Position of the simulated pinger. Defaults to origin. + /// Rate of simulated sensor messages. Defaults to 1Hz. + /// Noise model for the range to the simulated pinger. + /// Noise model for the bearing to the simulated pinger. + /// Noise model for the elevation to the simulated pinger. + class AcousticPingerPlugin + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate + { + /// \brief Constructor. + public: AcousticPingerPlugin(); + + /// \brief Destructor. + public: ~AcousticPingerPlugin() override = default; + + // Documentation inherited. + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) override; + + // Documentation inherited. + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer. + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; +} +#endif diff --git a/vrx_gz/src/vrx_gz/bridges.py b/vrx_gz/src/vrx_gz/bridges.py index 8f44e1e94..e3d79043a 100644 --- a/vrx_gz/src/vrx_gz/bridges.py +++ b/vrx_gz/src/vrx_gz/bridges.py @@ -34,7 +34,6 @@ def air_pressure(world_name, model_name, link_name='base_link'): ros_type='sensor_msgs/msg/FluidPressure', direction=BridgeDirection.GZ_TO_ROS) - def pose(model_name): return Bridge( gz_topic=f'/model/{model_name}/pose', @@ -84,6 +83,22 @@ def thrust_joint_pos(model_name, side): ros_type='std_msgs/msg/Float64', direction=BridgeDirection.ROS_TO_GZ) +def acoustic_pinger(model_name): + return Bridge( + gz_topic=f'{model_name}/pingers/pinger/range_bearing', + ros_topic=f'pingers/pinger/range_bearing', + gz_type='ignition.msgs.Param', + ros_type='ros_gz_interfaces/msg/ParamVec', + direction=BridgeDirection.GZ_TO_ROS) + +def set_acoustic_pinger(model_name): + return Bridge( + gz_topic=f'{model_name}/pingers/pinger/set_pinger_position', + ros_topic=f'pingers/pinger/set_pinger_position', + gz_type='ignition.msgs.Vector3d', + ros_type='geometry_msgs/msg/Vector3', + direction=BridgeDirection.ROS_TO_GZ) + def comms_tx(model_name): return Bridge( gz_topic='/broker/msgs', diff --git a/vrx_gz/src/vrx_gz/launch.py b/vrx_gz/src/vrx_gz/launch.py index 3a82ba2a0..8dea81c7d 100644 --- a/vrx_gz/src/vrx_gz/launch.py +++ b/vrx_gz/src/vrx_gz/launch.py @@ -30,6 +30,10 @@ import os +GYMKHANA_WORLDS = [ + 'gymkhana_task' +] + PERCEPTION_WORLDS = [ 'perception_task' ] diff --git a/vrx_gz/src/vrx_gz/model.py b/vrx_gz/src/vrx_gz/model.py index 21f7a6d77..a7e4d2823 100644 --- a/vrx_gz/src/vrx_gz/model.py +++ b/vrx_gz/src/vrx_gz/model.py @@ -93,6 +93,9 @@ def bridges(self, world_name): # thrust joint pos cmd vrx_gz.bridges.thrust_joint_pos(self.model_name, 'left'), vrx_gz.bridges.thrust_joint_pos(self.model_name, 'right'), + # Acoustic pinger + vrx_gz.bridges.acoustic_pinger(self.model_name), + vrx_gz.bridges.set_acoustic_pinger(self.model_name), ]) return [bridges, nodes, custom_launches] diff --git a/vrx_gz/worlds/gymkhana_task.sdf b/vrx_gz/worlds/gymkhana_task.sdf new file mode 100644 index 000000000..71fe3584a --- /dev/null +++ b/vrx_gz/worlds/gymkhana_task.sdf @@ -0,0 +1,507 @@ + + + + + + + 0.004 + 1.0 + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -478.1 148.2 13.2 0 0.25 2.94 + + 0.25 + 10000 + + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + false + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + 300 + 50 + 50 + 50 + floating + false + #777777 + + + + true + true + 4000000 + + + + false + + + + + + docked_collapsed + + + + + + + docked_collapsed + + + + + + + docked_collapsed + + + + false + + + + + + + + + + ogre2 + true + + + + + + + + + + + + + + + + + + false + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + + EARTH_WGS84 + ENU + -33.724223 + 150.679736 + 0.0 + 0.0 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 0 0 0.2 0 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/sydney_regatta + + + + Coast Waves + 0 0 0 0 0 0 + coast_waves + + + + platform + platform + + + + + post_0 + -535.916809 154.362869 0.675884 -0.17182 0.030464 -0.005286 + https://fuel.gazebosim.org/1.0/openrobotics/models/post + + + post_1 + -527.48999 153.854782 0.425844 -0.1365 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/post + + + post_2 + -544.832825 156.671951 0.499025 -0.162625 0 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/post + + + + + -531.063721 147.668579 1.59471 -0.068142 0 -0.1 + https://fuel.gazebosim.org/1.0/openrobotics/models/antenna + + + + + ground_station_0 + -540.796448 146.493744 1.671421 -0.00834 0.01824 1.301726 + https://fuel.gazebosim.org/1.0/openrobotics/models/ground_station + + + ground_station_1 + -537.622681 145.827896 1.681931 -0.00603 0.018667 1.301571 + https://fuel.gazebosim.org/1.0/openrobotics/models/ground_station + + + ground_station_2 + -534.550537 144.910400 1.720474 -0.004994 0.020798 1.301492 + https://fuel.gazebosim.org/1.0/openrobotics/models/ground_station + + + + + + + mb_marker_buoy_red + -528 191 0 0 1.57 0 + https://fuel.gazebosim.org/1.0/openrobotics/models/mb_marker_buoy_red + + + 1000 + 0.0 + 25.0 + 2.0 + + link + 0 0 -0.3 0 0 0 + + + 0.325 + 0.1 + + + + + 1000 1000 + 50 50> + + PMS + 5.0 + 3 + 1.1 + 0.3 + 1 0 + 0.4 + 2.0 + 0.0 + 0.0 + + + + + + + 0 0 0 + + + + + 0.002 + + + 10 + + 0.05 + 60 + + + 0 + 0.0002 + + + + 30 + + 5 + 20 + + + 0 + 0.03 + + + + + + 0 + 0.03 + + + + + + + wamv + station_keeping + /vrx/task/info + 10 + 10 + 300 + /vrx/release + + -33.722718 150.674031 0 + + + + + + diff --git a/vrx_gz/worlds/stationkeeping_task.sdf b/vrx_gz/worlds/stationkeeping_task.sdf index 3b118badc..d1fe37c7d 100644 --- a/vrx_gz/worlds/stationkeeping_task.sdf +++ b/vrx_gz/worlds/stationkeeping_task.sdf @@ -680,7 +680,7 @@ - +