Skip to content

Commit

Permalink
Implemented current profiling in the DVL sensor & plugin
Browse files Browse the repository at this point in the history 
The package is dependent on the NPS version of the UUV Simulator
ocean current plugin. If stratified current is being provided by the
UUV Simulator ocean current plugin, it will be ingested by the DVL
sensor.  The DVL sensor will use this information to calculate
noise-free scalar beam velocity at the center of each "depth bind"
for all 4 DVL beams (velocity beyond the beam's "contact" range is
set to a "no value" flag).  The DVL ROS plugin adds noise to the
velocities and computes and publishes either beam-specific 3D
velocities (i.e., scalar velocity times the beam's unit vector) or
overall velocities (i.e., solves a least-squares problem based on
the noisy beam velocities) using the ds_sensor_msgs/Adcp message.
  • Loading branch information
@dtdavi1
dtdavi1 committed Sep 21, 2021
1 parent a3a2c57 commit 52df7c9
Show file tree
Hide file tree
Showing 5 changed files with 199 additions and 25 deletions.
9 changes: 5 additions & 4 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
geometry_msgs
gazebo_ros
tf
dave_gazebo_world_plugins
)

find_package(ClangFormat)
Expand Down Expand Up @@ -85,8 +86,8 @@ set(SENSORS_SRCS
# dsros_sensors (passed at STARTUP as a system plugin)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_library(dsros_sensors ${SENSORS_SRCS})
target_link_libraries(dsros_sensors ds_sim_gazebo_msgs ${GAZEBO_LIBRARIES})
add_dependencies(dsros_sensors ds_sim_gazebo_msgs)
target_link_libraries(dsros_sensors ds_sim_gazebo_msgs dave_gazebo_world_plugins_msgs ${GAZEBO_LIBRARIES})
add_dependencies(dsros_sensors ds_sim_gazebo_msgs dave_gazebo_world_plugins_msgs)

# dsros_ros_depth
add_library(dsros_ros_depth src/dsros_depth_plugin.cc src/dsros_depth_plugin.hh)
Expand All @@ -100,8 +101,8 @@ add_dependencies(dsros_ros_ins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPO

# DVL plugin
add_library(dsros_ros_dvl src/dsros_dvl_plugin.cc src/dsros_dvl_plugin.hh)
target_link_libraries(dsros_ros_dvl dsros_sensors ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
add_dependencies(dsros_ros_dvl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(dsros_ros_dvl dsros_sensors dave_gazebo_world_plugins_msgs ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
add_dependencies(dsros_ros_dvl dave_gazebo_world_plugins_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

# GPS plugin
add_library(dsros_ros_gps src/dsros_gps_plugin.cc src/dsros_gps_plugin.hh)
Expand Down
139 changes: 138 additions & 1 deletion gazebo_src/dsros_dvl.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ DsrosDvlBeam::DsrosDvlBeam(const physics::PhysicsEnginePtr& physicsEngine,
const ignition::math::Pose3d& sensor_pose,
const ignition::math::Pose3d& beam_pose) {

this->parent = parent;

// Compute the center pose
double range = parent->RangeMax();
double radius = range * tan(parent->BeamWidth()*M_PI/180.0);
Expand Down Expand Up @@ -101,6 +103,8 @@ DsrosDvlBeam::DsrosDvlBeam(const physics::PhysicsEnginePtr& physicsEngine,
this->collision->SetCollideBits(GZ_SENSOR_COLLIDE);
this->collision->SetCategoryBits(GZ_SENSOR_COLLIDE);

this->beamWaterVelocityBins.resize(parent->waterTrackBins);

// setup the contact subscription
/*
std::string topic =
Expand All @@ -127,9 +131,39 @@ void DsrosDvlBeam::Update(const physics::WorldPtr& world,
contactRange += startRange; // have to add the initial range offset. #bugfix.

// if no bottom return, use sensor water velocity to compute the beam's water track velocity
// NOTE: This is based on the "global" current, not the stratified current
ignition::math::Vector3d inst_wtr_vel = inst2world.Rot().RotateVectorReverse(sensorWtrVel);
beamWaterVelocity = beamUnitVector.Dot(inst_wtr_vel);

// Use stratified current to compute bean-specific velocities for each bin
for (int bin = 0; bin < this->beamWaterVelocityBins.size(); bin++)
{
double binRange = this->parent->currentProfileBin0Distance +
bin * this->parent->currentProfileCellDepth;
if ((this->contactRange > 0) && (binRange > this->contactRange))
{
this->beamWaterVelocityBins[bin] = DsrosDvlBeam::NO_VELOCITY;
}
else
{
ignition::math::Vector3d binPt =
inst2world.Rot().RotateVector(binRange * beamUnitVector) +
inst2world.Pos();
ignition::math::Vector3d current =
this->parent->OceanCurrentAtDepth(-binPt.Z());
ignition::math::Vector3d binVelocity =
inst2world.Rot().RotateVectorReverse(sensorVel - current);
this->beamWaterVelocityBins[bin] = this->beamUnitVector.Dot(binVelocity);
// gzmsg << "Beam " << this->number << " bin " << bin
// << " water velocity info:" << std::endl;
// gzmsg << " Bin depth: " << binRange << std::endl;
// gzmsg << " Bin point: " << binPt << std::endl;
// gzmsg << " Current at bin point depth: " << current << std::endl;
// gzmsg << " Bin velocity: " << binVelocity << " and value: "
// << this->beamWaterVelocityBins[bin] << std::endl;
}
}

if (entityName.empty()) {
contactEntityName = "";
contactEntityPtr.reset();
Expand Down Expand Up @@ -243,6 +277,22 @@ void DsrosDvlSensor::Load(const std::string &_worldName) {
this->beamWidth = 4.0;
*/

// Set up water tracking/current profiling bins
if (dvlElem->HasElement("waterTrackBins"))
{
this->waterTrackBins = dvlElem->Get<int>("waterTrackBins");
// gzmsg << "Water track bins set to " << this->waterTrackBins << std::endl;
}
else
{
this->waterTrackBins = 1;
// gzmsg << "Water track bins set to default" << std::endl;
}
this->currentProfileCellDepth = (this->rangeMax - this->rangeMin) /
this->waterTrackBins;
this->currentProfileBin0Distance = this->rangeMin +
this->currentProfileCellDepth / 2.0;

// setup to publish!
this->topicName = this->GetTopic();
this->dvlPub = this->node->Advertise<ds_sim::msgs::Dvl>(this->topicName, 50);
Expand Down Expand Up @@ -283,6 +333,12 @@ void DsrosDvlSensor::Load(const std::string &_worldName) {
this->currentTopicName = this->GetOceanCurrentTopic();
this->currentSub = this->node->Subscribe(this->currentTopicName,
&DsrosDvlSensor::OnOceanCurrent, this);

// setup the stratified ocean current subscription
this->stratifiedCurrentTopicName = this->GetStratifiedOceanCurrentTopic();
this->stratifiedCurrentSub =
this->node->Subscribe(this->stratifiedCurrentTopicName,
&DsrosDvlSensor::OnStratifiedOceanCurrent, this);
}

void DsrosDvlSensor::Init() {
Expand Down Expand Up @@ -323,6 +379,20 @@ std::string DsrosDvlSensor::GetOceanCurrentTopic() const {
return topicName;
}

std::string DsrosDvlSensor::GetStratifiedOceanCurrentTopic() const {
std::string topicName = "";

if (this->sdf->HasElement("stratified_ocean_current_topic")) {
topicName += this->sdf->Get<std::string>("stratified_ocean_current_topic");
} else {
topicName += "hydrodynamics/stratified_current_velocity";
}

boost::replace_all(topicName, "::", "/");

return topicName;
}

common::Time DsrosDvlSensor::GetTime() const {
std::lock_guard<std::mutex>(this->mutex);
return msgs::Convert(this->msg.stamp());
Expand Down Expand Up @@ -388,6 +458,10 @@ double DsrosDvlSensor::GetBeamWaterVelocity(int idx) const {
return beams[idx].beamWaterVelocity;
}

double DsrosDvlSensor::GetBeamWaterVelocityBin(int idx, int bin) const {
return beams[idx].beamWaterVelocityBins[bin];
}

double DsrosDvlSensor::GetBeamRange(int idx) const {
return beams[idx].contactRange;
}
Expand All @@ -396,7 +470,22 @@ void DsrosDvlSensor::OnOceanCurrent(ConstVector3dPtr &_msg) {
this->oceanCurrent.X() = _msg->x();
this->oceanCurrent.Y() = _msg->y();
this->oceanCurrent.Z() = _msg->z();
//gzmsg << "Update ocean current to " << this->oceanCurrent << "\n";
gzmsg << "Update ocean current to " << this->oceanCurrent << "\n";
}

void DsrosDvlSensor::OnStratifiedOceanCurrent(
ConstStratifiedCurrentVelocityPtr &_msg) {
stratifiedOceanCurrent.clear();
for (int i=0; i < _msg->velocity_size(); i++) {
ignition::math::Vector4d vel(_msg->velocity(i).x(),
_msg->velocity(i).y(),
_msg->velocity(i).z(),
_msg->depth(i));
this->stratifiedOceanCurrent.push_back(vel);
// gzmsg << "Stratified current at " << vel.W()
// << " meters: (" << vel.X() << ", " << vel.Y() << ", "
// << vel.Z() << ")" << std::endl;
}
}

bool DsrosDvlSensor::UpdateImpl(const bool _force) {
Expand Down Expand Up @@ -526,5 +615,53 @@ bool DsrosDvlSensor::UpdateImpl(const bool _force) {
return true;
}

ignition::math::Vector3d DsrosDvlSensor::OceanCurrentAtDepth(double depth) const
{
ignition::math::Vector3d current(0.0, 0.0, 0.0);
// If test depth is above the surface, just return (0, 0, 0)
if (depth < 0.0)
{
return current;
}

// If no stratified current, return the single current value
if (this->stratifiedOceanCurrent.size() == 0)
{
return this->oceanCurrent;
}

// Shallower than first depth (return first depth value)
if (depth <= this->stratifiedOceanCurrent.front().W())
{
ignition::math::Vector4d stratCurrent =
this->stratifiedOceanCurrent.front();
current.Set(stratCurrent.X(), stratCurrent.Y(), stratCurrent.Z());
}

// Deeper than last depth (return last depth value)
if (depth >= this->stratifiedOceanCurrent.back().W())
{
ignition::math::Vector4d stratCurrent =
this->stratifiedOceanCurrent.front();
current.Set(stratCurrent.X(), stratCurrent.Y(), stratCurrent.Z());
}

double lowIndex = 0;
ignition::math::Vector4d lower = this->stratifiedOceanCurrent[lowIndex];
ignition::math::Vector4d upper = this->stratifiedOceanCurrent[lowIndex+1];
while (depth > upper.W())
{
lowIndex++;
lower = upper;
upper = this->stratifiedOceanCurrent[lowIndex+1];
}
double interp = (depth - lower.W()) / (upper.W() - lower.W());
double xCurrent = lower.X() + interp * (upper.X() - lower.X());
double yCurrent = lower.Y() + interp * (upper.Y() - lower.Y());
double zCurrent = lower.Z() + interp * (upper.Z() - lower.Z());
current.Set(xCurrent, yCurrent, zCurrent);
return current;
}

GZ_REGISTER_STATIC_SENSOR("dsros_dvl", DsrosDvlSensor);

27 changes: 27 additions & 0 deletions gazebo_src/dsros_dvl.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@

#include <string>
#include <mutex>
#include <cfloat>

#include <sdf/sdf.hh>
#include <ignition/math/Vector3.hh>
Expand All @@ -56,10 +57,16 @@
#include <gazebo/common/common.hh>

#include <SensorDvl.pb.h>
#include <StratifiedCurrentVelocity.pb.h>

namespace gazebo {
namespace sensors {

typedef const boost::shared_ptr<
const dave_gazebo_world_plugins_msgs::msgs::StratifiedCurrentVelocity>
ConstStratifiedCurrentVelocityPtr;


// The gazebo implementation uses pimpl, but we can
// easily recompile our plugin, so don't bother.

Expand All @@ -68,6 +75,7 @@ namespace sensors {

class DsrosDvlBeam {
public:
static constexpr double NO_VELOCITY = DBL_MAX; // "No current velocity" flag value

DsrosDvlBeam(const physics::PhysicsEnginePtr& physicsEngine,
const DsrosDvlSensor* parent, int beamnum,
Expand All @@ -78,6 +86,7 @@ namespace sensors {
void Update(const physics::WorldPtr& world, const ignition::math::Vector3d& sensorVel,
const ignition::math::Vector3d& sensorWtrVel,
const ignition::math::Pose3d& inst2world);
const DsrosDvlSensor* parent;
physics::CollisionPtr collision;
physics::RayShapePtr shape;
ignition::math::Pose3d centerPose;
Expand All @@ -91,6 +100,8 @@ namespace sensors {
double startRange;
double beamVelocity;
double beamWaterVelocity;

std::vector<double> beamWaterVelocityBins;
ignition::math::Vector3d beamUnitVector; // in body coordinates

bool isValid() const;
Expand All @@ -110,6 +121,8 @@ namespace sensors {

public: virtual std::string GetOceanCurrentTopic() const;

public: virtual std::string GetStratifiedOceanCurrentTopic() const;

public: virtual void Init();

public: virtual void Fini();
Expand All @@ -122,6 +135,7 @@ namespace sensors {
public: bool BeamValid(int idx) const;
public: double GetBeamVelocity(int idx) const;
public: double GetBeamWaterVelocity(int idx) const;
public: double GetBeamWaterVelocityBin(int idx, int bin) const;
public: double GetBeamRange(int idx) const;

public: double RangeMin() const;
Expand All @@ -136,6 +150,9 @@ namespace sensors {
public: friend class DsrosDvlBeam;
protected: virtual bool UpdateImpl(const bool _force);
protected: void OnOceanCurrent(ConstVector3dPtr &_msg);
protected: void OnStratifiedOceanCurrent(ConstStratifiedCurrentVelocityPtr &_msg);

private: ignition::math::Vector3d OceanCurrentAtDepth(double depth) const;

protected:
physics::LinkPtr parentLink;
Expand All @@ -160,6 +177,11 @@ namespace sensors {

std::vector<DsrosDvlBeam> beams;

// Water track/current profiling bin information
int waterTrackBins;
double currentProfileCellDepth;
double currentProfileBin0Distance;

// Collision physics stuff
physics::CollisionPtr rangeCollision;
physics::MeshShapePtr beamShape;
Expand All @@ -169,6 +191,11 @@ namespace sensors {
std::string currentTopicName;
transport::SubscriberPtr currentSub;
ignition::math::Vector3d oceanCurrent;

// Stratified ocean current subscription (if it's being published)
std::string stratifiedCurrentTopicName;
transport::SubscriberPtr stratifiedCurrentSub;
std::vector<ignition::math::Vector4d> stratifiedOceanCurrent;
}; // class declaration
}; // namespace sensors
}; // namespace gazebo
Expand Down
3 changes: 3 additions & 0 deletions package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,14 +52,17 @@
<build_depend>ds_core_msgs</build_depend>
<build_depend>ds_sensor_msgs</build_depend>
<build_depend>ds_nav_msgs</build_depend>
<build_depend>dave_gazebo_world_plugins</build_depend>
<build_export_depend>ds_actuator_msgs</build_export_depend>
<build_export_depend>ds_core_msgs</build_export_depend>
<build_export_depend>ds_sensor_msgs</build_export_depend>
<build_export_depend>ds_nav_msgs</build_export_depend>
<build_export_depend>dave_gazebo_world_plugins</build_export_depend>
<exec_depend>ds_actuator_msgs</exec_depend>
<exec_depend>ds_core_msgs</exec_depend>
<exec_depend>ds_sensor_msgs</exec_depend>
<exec_depend>ds_nav_msgs</exec_depend>
<exec_depend>dave_gazebo_world_plugins</exec_depend>

<!-- The export tag contains other, unspecified, tags -->
<export>
Expand Down
Loading

0 comments on commit 52df7c9

Please sign in to comment.