Skip to content

Commit

Permalink
Added crude "water tracking" capability to DVL plugin
Browse files Browse the repository at this point in the history 
Selectable via the plugin definition <enableWaterTrack> element in
the plugin definition (default to disabled).  The sensor keeps track
of both water track and bottom track velocities.  Water track velocity
is currently based on linear velocity and does not account for any
sort of current (possible future work).  When enabled, the plugin
will use bottom track if it is available (>= 3 valid beams) and water
track if it is not.

Also updated the plugin to add the raw_velocity and raw_velocity_covar
values to the ROS message (will use bottom track values if they are
available and water track values if they are not).
  • Loading branch information
@dtdavi1
dtdavi1 committed Sep 21, 2021
1 parent cc19f57 commit 7bba36f
Show file tree
Hide file tree
Showing 4 changed files with 91 additions and 13 deletions.
36 changes: 34 additions & 2 deletions gazebo_src/dsros_dvl.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,11 +125,15 @@ void DsrosDvlBeam::Update(const physics::WorldPtr& world,
this->shape->GetIntersection(contactRange, entityName);
contactRange += startRange; // have to add the initial range offset. #bugfix.

// if no bottom return, use sensor velocity to estimate a water track velocity
// TODO: incorporate current?
ignition::math::Vector3d inst_wtr_vel = inst2world.Rot().RotateVectorReverse(sensorVel);
beamWaterVelocity = beamUnitVector.Dot(inst_wtr_vel);

if (entityName.empty()) {
contactEntityName = "";
contactEntityPtr.reset();
contactRange = -1;
beamVelocity = 0;
return;
}

Expand Down Expand Up @@ -216,7 +220,7 @@ void DsrosDvlSensor::Load(const std::string &_worldName) {
}

if (!dvlElem) {
gzerr <<"DVL sensor MUST have plugin with filename=libdsros_ros_dvl.so so we can get min/max range, etc\n";
//gzdbg <<"DVL sensor MUST have plugin with filename=libdsros_ros_dvl.so so we can get min/max range, etc\n";
throw std::runtime_error("DVL Sensor MUST specify a plugin with parameters");
}

Expand All @@ -230,6 +234,7 @@ void DsrosDvlSensor::Load(const std::string &_worldName) {
this->beamAzimuth3 = dvlElem->Get<double>("beamAzimuthDeg3");
this->beamAzimuth4 = dvlElem->Get<double>("beamAzimuthDeg4");
this->pos_z_down = dvlElem->Get<bool>("pos_z_down");

/*
this->rangeMin = 1.0;
this->rangeMax = 200;
Expand Down Expand Up @@ -360,6 +365,10 @@ double DsrosDvlSensor::GetBeamVelocity(int idx) const {
return beams[idx].beamVelocity;
}

double DsrosDvlSensor::GetBeamWaterVelocity(int idx) const {
return beams[idx].beamWaterVelocity;
}

double DsrosDvlSensor::GetBeamRange(int idx) const {
return beams[idx].contactRange;
}
Expand Down Expand Up @@ -400,6 +409,8 @@ bool DsrosDvlSensor::UpdateImpl(const bool _force) {
int basis_fill_in = 0;
Eigen::Matrix<double, Eigen::Dynamic, 3> beam_basis(4,3);
Eigen::VectorXd beam_vel(4);
Eigen::Matrix<double, Eigen::Dynamic, 3> beam_wtr_basis(4,3);
Eigen::VectorXd beam_wtr_vel(4);
for (size_t i=0; i<beams.size(); i++) {
beams[i].Update(world, sensorVel, sensorPose);
//msg <<beams[i].contactRange <<"/" <<beams[i].beamVelocity <<" ";
Expand All @@ -411,6 +422,12 @@ bool DsrosDvlSensor::UpdateImpl(const bool _force) {
beam_vel(basis_fill_in) = beams[i].beamVelocity;
basis_fill_in++;
}
// compute in water track values even if no bottom return
beam_wtr_basis(i,0) = beams[i].beamUnitVector.X();
beam_wtr_basis(i,1) = beams[i].beamUnitVector.Y();
beam_wtr_basis(i,2) = beams[i].beamUnitVector.Z();
beam_wtr_vel(i) = beams[i].beamWaterVelocity;
gzerr << "beam[" << i << "] vel: " << beams[i].beamVelocity <<", wtr vel: " <<beams[i].beamWaterVelocity <<"\n";
}
//gzdbg <<msg.str() <<" valid: " <<valid_beams <<"\n";

Expand All @@ -433,6 +450,21 @@ bool DsrosDvlSensor::UpdateImpl(const bool _force) {
ignition::math::Vector3d stefVel = sensorPose.Rot().RotateVector(linear_velocity);
ignition::math::Vector3d bodyVel = vehPose.Rot().RotateVectorReverse(bodyLinearVel);

//gzdbg <<" comp. vel: (" <<inst_vel(0) <<"," <<inst_vel(1) <<"," <<inst_vel(2) <<")\n"
// <<" orig. vel: (" <<bodyVel.X() <<"," <<bodyVel.Y() <<"," <<bodyVel.Z() <<")\n"
// <<" stef. vel: (" <<stefVel.X() <<"," <<stefVel.Y() <<"," <<stefVel.Z() <<")\n";
} else {
// same approach for a water track solution, but use the beamWaterVelocity values
// assumes all beams are "valid" as far as the water track solution goes
Eigen::MatrixXd H(beam_wtr_basis);
Eigen::Vector3d inst_vel = H.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(beam_wtr_vel);

linear_velocity.X( inst_vel(0) );
linear_velocity.Y( inst_vel(1) );
linear_velocity.Z( inst_vel(2) );

ignition::math::Vector3d stefVel = sensorPose.Rot().RotateVector(linear_velocity);
ignition::math::Vector3d bodyVel = vehPose.Rot().RotateVectorReverse(bodyLinearVel);
//gzdbg <<" comp. vel: (" <<inst_vel(0) <<"," <<inst_vel(1) <<"," <<inst_vel(2) <<")\n"
// <<" orig. vel: (" <<bodyVel.X() <<"," <<bodyVel.Y() <<"," <<bodyVel.Z() <<")\n"
// <<" stef. vel: (" <<stefVel.X() <<"," <<stefVel.Y() <<"," <<stefVel.Z() <<")\n";
Expand Down
2 changes: 2 additions & 0 deletions gazebo_src/dsros_dvl.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,7 @@ namespace sensors {
double contactRange;
double startRange;
double beamVelocity;
double beamWaterVelocity;
ignition::math::Vector3d beamUnitVector; // in body coordinates

bool isValid() const;
Expand Down Expand Up @@ -117,6 +118,7 @@ namespace sensors {
public: ignition::math::Vector3d GetBeamUnitVec(int idx) const;
public: bool BeamValid(int idx) const;
public: double GetBeamVelocity(int idx) const;
public: double GetBeamWaterVelocity(int idx) const;
public: double GetBeamRange(int idx) const;

public: double RangeMin() const;
Expand Down
65 changes: 54 additions & 11 deletions src/dsros_dvl_plugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,19 +91,26 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) {

// add noise and recompute the velocity
std::vector<double> ranges(4);
Eigen::VectorXd raw_beam_vel(4);
Eigen::VectorXd beam_vel(4);
Eigen::VectorXd beam_wtr_vel(4);
Eigen::MatrixXd beam_unit(4,3);
Eigen::MatrixXd beam_wtr_unit(4,3);
Eigen::Vector3d velocity;
Eigen::Vector3d wtr_velocity;
double speed = 0;
double course = 0;
double altitude = 0;
double wtr_speed = 0;
double wtr_course = 0;
int fillIn = 0;
for (size_t i=0; i<sensor->NumBeams(); i++) {

ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(i);
if (sensor->BeamValid(i)) {
ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(i);
ranges[i] = sensor->GetBeamRange(i) + GaussianKernel(0, gaussian_noise_range);
beam_vel(fillIn) = sensor->GetBeamVelocity(i) + GaussianKernel(0, gaussian_noise_vel);
raw_beam_vel(i) = sensor->GetBeamVelocity(i) + GaussianKernel(0, gaussian_noise_vel);
beam_vel(fillIn) = raw_beam_vel(i);
beam_unit(fillIn, 0) = beamUnit.X();
beam_unit(fillIn, 1) = beamUnit.Y();
beam_unit(fillIn, 2) = beamUnit.Z();
Expand All @@ -112,7 +119,12 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) {
} else {
ranges[i] = std::numeric_limits<double>::quiet_NaN();
}
beam_wtr_vel(i) = sensor->GetBeamWaterVelocity(i) + GaussianKernel(0, gaussian_noise_vel);
beam_wtr_unit(i, 0) = beamUnit.X();
beam_wtr_unit(i, 1) = beamUnit.Y();
beam_wtr_unit(i, 2) = beamUnit.Z();
}

if (fillIn >= 3) {
// trim the arrays
beam_unit = beam_unit.topRows(fillIn);
Expand All @@ -130,8 +142,13 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) {
course += 360.0;
}
}


// same method to compute a water track velocity based on the noisy beam water velocities
wtr_velocity = beam_wtr_unit.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(beam_wtr_vel);
wtr_speed = sqrt(wtr_velocity(0)*wtr_velocity(0) + wtr_velocity(1)*wtr_velocity(1));
wtr_course = atan2(wtr_velocity(0), wtr_velocity(1)) * 180.0/M_PI;
if (wtr_course < 0) {
wtr_course += 360.0;
}

if(dvl_data_publisher.getNumSubscribers() > 0) {

Expand All @@ -144,10 +161,21 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) {
msg.ds_header.io_time.sec = current_time.sec;
msg.ds_header.io_time.nsec = current_time.nsec;

msg.velocity.x = velocity(0);
msg.velocity.y = velocity(1);
msg.velocity.z = velocity(2);

if ((fillIn >= 3) || (!water_track_enabled)) {
msg.velocity.x = velocity(0);
msg.velocity.y = velocity(1);
msg.velocity.z = velocity(2);
msg.course_gnd = course;
msg.speed_gnd = speed;
msg.velocity_mode = ds_sensor_msgs::Dvl::DVL_MODE_BOTTOM;
} else {
msg.velocity.x = wtr_velocity(0);
msg.velocity.y = wtr_velocity(1);
msg.velocity.z = wtr_velocity(2);
msg.course_gnd = wtr_course;
msg.speed_gnd = wtr_speed;
msg.velocity_mode = ds_sensor_msgs::Dvl::DVL_MODE_WATER;
}
msg.velocity_covar[0] = gaussian_noise_vel*gaussian_noise_vel;
msg.velocity_covar[1] = 0;
msg.velocity_covar[2] = 0;
Expand All @@ -162,8 +190,6 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) {

msg.num_good_beams = sensor->ValidBeams();
msg.speed_sound = 1500.0;
msg.course_gnd = course;
msg.speed_gnd = speed;
msg.altitude = altitude;

for (size_t i=0; i<sensor->NumBeams(); i++) {
Expand All @@ -173,11 +199,16 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) {
msg.beam_unit_vec[i].x = beamUnit.X();
msg.beam_unit_vec[i].y = beamUnit.Y();
msg.beam_unit_vec[i].z = beamUnit.Z();
if (fillIn >= 3) {
msg.raw_velocity[i] = raw_beam_vel(i);
} else {
msg.raw_velocity[i] = beam_wtr_vel(i);
}
msg.raw_velocity_covar[i] = gaussian_noise_vel*gaussian_noise_vel;
}

//ROS_INFO_STREAM("DVL_SENDING_INST: " <<velocity(0) <<" " <<velocity(1) <<" " <<velocity(2));

msg.velocity_mode = ds_sensor_msgs::Dvl::DVL_MODE_BOTTOM;
msg.coordinate_mode = ds_sensor_msgs::Dvl::DVL_COORD_INSTRUMENT;
msg.dvl_time = static_cast<double>(current_time.sec) + static_cast<double>(current_time.nsec)/1.0e9;

Expand Down Expand Up @@ -369,5 +400,17 @@ bool dsrosRosDvlSensor::LoadParameters() {
gaussian_noise_range = 0.0;
ROS_WARN_STREAM("missing <gaussianNoiseBeamRange>, set to default: " << gaussian_noise_range);
}

//WATER TRACKING
if (sdf->HasElement("enableWaterTrack"))
{
water_track_enabled = sdf->Get<bool>("enableWaterTrack");
ROS_INFO_STREAM("<enableWaterTrack> set to: " << water_track_enabled);
}
else
{
water_track_enabled = false;
ROS_WARN_STREAM("missing <enableWaterTrack>, set to default: " << water_track_enabled);
}
return true;
}
1 change: 1 addition & 0 deletions src/dsros_dvl_plugin.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,7 @@ private:
double update_rate;
double gaussian_noise_vel;
double gaussian_noise_range;
bool water_track_enabled;
};

}; // namespace gazebo

0 comments on commit 7bba36f

Please sign in to comment.