diff --git a/CMakeLists.txt b/CMakeLists.txt index 1012a9e..1a07fd6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs gazebo_ros tf + dave_gazebo_world_plugins ) find_package(ClangFormat) @@ -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) @@ -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) diff --git a/gazebo_src/dsros_dvl.cc b/gazebo_src/dsros_dvl.cc index 1f85f3d..e458fe0 100644 --- a/gazebo_src/dsros_dvl.cc +++ b/gazebo_src/dsros_dvl.cc @@ -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); @@ -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 = @@ -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(); @@ -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("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(this->topicName, 50); @@ -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() { @@ -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("stratified_ocean_current_topic"); + } else { + topicName += "hydrodynamics/stratified_current_velocity"; + } + + boost::replace_all(topicName, "::", "/"); + + return topicName; +} + common::Time DsrosDvlSensor::GetTime() const { std::lock_guard(this->mutex); return msgs::Convert(this->msg.stamp()); @@ -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; } @@ -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) { @@ -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); diff --git a/gazebo_src/dsros_dvl.hh b/gazebo_src/dsros_dvl.hh index b563e09..7c7d1ad 100644 --- a/gazebo_src/dsros_dvl.hh +++ b/gazebo_src/dsros_dvl.hh @@ -43,6 +43,7 @@ #include #include +#include #include #include @@ -56,10 +57,16 @@ #include #include +#include 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. @@ -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, @@ -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; @@ -91,6 +100,8 @@ namespace sensors { double startRange; double beamVelocity; double beamWaterVelocity; + + std::vector beamWaterVelocityBins; ignition::math::Vector3d beamUnitVector; // in body coordinates bool isValid() const; @@ -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(); @@ -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; @@ -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; @@ -160,6 +177,11 @@ namespace sensors { std::vector beams; + // Water track/current profiling bin information + int waterTrackBins; + double currentProfileCellDepth; + double currentProfileBin0Distance; + // Collision physics stuff physics::CollisionPtr rangeCollision; physics::MeshShapePtr beamShape; @@ -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 stratifiedOceanCurrent; }; // class declaration }; // namespace sensors }; // namespace gazebo diff --git a/package.xml b/package.xml index 95782f3..4b14a71 100644 --- a/package.xml +++ b/package.xml @@ -52,14 +52,17 @@ ds_core_msgs ds_sensor_msgs ds_nav_msgs + dave_gazebo_world_plugins ds_actuator_msgs ds_core_msgs ds_sensor_msgs ds_nav_msgs + dave_gazebo_world_plugins ds_actuator_msgs ds_core_msgs ds_sensor_msgs ds_nav_msgs + dave_gazebo_world_plugins diff --git a/src/dsros_dvl_plugin.cc b/src/dsros_dvl_plugin.cc index 60fe910..11673f0 100644 --- a/src/dsros_dvl_plugin.cc +++ b/src/dsros_dvl_plugin.cc @@ -85,15 +85,15 @@ void dsrosRosDvlSensor::Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) { // Initialize data members and the Adcp message for current profiling current_profile_cell_depth = (sensor->RangeMax() - sensor->RangeMin()) / water_track_bins; current_profile_bin0_distance = sensor->RangeMin() + current_profile_cell_depth / 2.0; - for (int beam = 0; beam < 4; beam++) + for (size_t beam = 0; beam < 4; beam++) { ignition::math::Vector3d unit_vec = sensor->GetBeamUnitVec(beam); adcp.beam_unit_vec[beam].x = unit_vec.X(); adcp.beam_unit_vec[beam].y = unit_vec.Y(); adcp.beam_unit_vec[beam].z = unit_vec.Z(); - } // for (int beam = 0;... + } // for (size_t beam = 0;... adcp.vel_bin_beams.resize(water_track_bins); - for (int bin = 0; bin < water_track_bins; bin++) + for (size_t bin = 0; bin < water_track_bins; bin++) { if (current_profile_coord_mode == ds_sensor_msgs::Adcp::ADCP_COORD_BEAM) { @@ -107,7 +107,7 @@ void dsrosRosDvlSensor::Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) { // adcp.vel_bin_beams[bin].bin_intensity.resize(1); // Commented out in msg def for now // adcp.vel_bin_beams[bin].bin_correlation.resize(1); // Commented out in msg def for now } - } // for (int bin = 0;... + } // for (size_t bin = 0;... } void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { @@ -179,6 +179,7 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { wtr_course += 360.0; } + // publish DVL data message if(dvl_data_publisher.getNumSubscribers() > 0) { // prepare message header @@ -257,6 +258,7 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { ros::spinOnce(); } + // publish current profile (ADCP) message if ((current_profile_publisher.getNumSubscribers() > 0) && ((current_profile_coord_mode == ds_sensor_msgs::Adcp::ADCP_COORD_BEAM) || (current_profile_coord_mode == ds_sensor_msgs::Adcp::ADCP_COORD_INSTRUMENT)) && @@ -279,25 +281,24 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { // Fill in beam-specific bin velocities as water velocity plus noise // out to the beam's current range. No solution (0.0) beyond that - for (int bin = 0; bin < water_track_bins; bin ++) + for (size_t bin = 0; bin < water_track_bins; bin ++) { double bin_range = current_profile_bin0_distance + - (current_profile_cell_depth / 2.0) + (current_profile_cell_depth * bin); if (current_profile_coord_mode == ds_sensor_msgs::Adcp::ADCP_COORD_BEAM) { // Calculate a velocity in beam coordinates for each beam for every cell for (size_t beam=0; beam < sensor->NumBeams(); beam++) { - double beam_range = sensor->GetBeamRange(beam); - if ((beam_range >= bin_range) || (beam_range <= 0.0)) + // add noise to the beam's bin-specific velocity + double bin_velocity = sensor->GetBeamWaterVelocityBin(beam, bin) + + GaussianKernel(0, gaussian_noise_wtr_vel); + if (bin_velocity != gazebo::sensors::DsrosDvlBeam::NO_VELOCITY) { ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(beam); - double beam_velocity = sensor->GetBeamWaterVelocity(beam) + - GaussianKernel(0, gaussian_noise_wtr_vel); - adcp.vel_bin_beams[bin].velocity_bin_beam[beam].x = -beam_velocity * beamUnit.X(); - adcp.vel_bin_beams[bin].velocity_bin_beam[beam].y = -beam_velocity * beamUnit.Y(); - adcp.vel_bin_beams[bin].velocity_bin_beam[beam].z = -beam_velocity * beamUnit.Z(); + adcp.vel_bin_beams[bin].velocity_bin_beam[beam].x = -bin_velocity * beamUnit.X(); + adcp.vel_bin_beams[bin].velocity_bin_beam[beam].y = -bin_velocity * beamUnit.Y(); + adcp.vel_bin_beams[bin].velocity_bin_beam[beam].z = -bin_velocity * beamUnit.Z(); } // if (beam_range >=... else { @@ -312,11 +313,13 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { bool range_solution = true; for (size_t beam=0; beam < sensor->NumBeams(); beam++) { - double beam_range = sensor->GetBeamRange(beam); - if (range_solution && ((beam_range >= bin_range) || (beam_range <= 0.0))) + double bin_velocity = sensor->GetBeamWaterVelocityBin(beam, bin); + if (range_solution && + (bin_velocity != gazebo::sensors::DsrosDvlBeam::NO_VELOCITY)) { ignition::math::Vector3d beamUnit = sensor->GetBeamUnitVec(beam); - beam_wtr_vel(beam) = sensor->GetBeamWaterVelocity(beam) + GaussianKernel(0, gaussian_noise_wtr_vel); + beam_wtr_vel(beam) = sensor->GetBeamWaterVelocityBin(beam, bin) + + GaussianKernel(0, gaussian_noise_wtr_vel); beam_wtr_unit(beam, 0) = beamUnit.X(); beam_wtr_unit(beam, 1) = beamUnit.Y(); beam_wtr_unit(beam, 2) = beamUnit.Z(); @@ -328,7 +331,10 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { } // for (size_t beam=0;... if (range_solution) // if bin for any beam is beyond bottom, solution will be 0 { - Eigen::Vector3d bin_velocity = beam_wtr_unit.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(beam_wtr_vel); + Eigen::Vector3d bin_velocity = + beam_wtr_unit.jacobiSvd(Eigen::ComputeThinU | + Eigen::ComputeThinV). + solve(beam_wtr_vel); adcp.vel_bin_beams[bin].velocity_bin_beam[0].x = -bin_velocity[0]; adcp.vel_bin_beams[bin].velocity_bin_beam[0].y = -bin_velocity[1]; adcp.vel_bin_beams[bin].velocity_bin_beam[0].z = -bin_velocity[2]; @@ -340,13 +346,13 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { adcp.vel_bin_beams[bin].velocity_bin_beam[0].z = 0.0; } // else } // else - } // for (int bin = 0;... + } // for (size_t bin = 0;... current_profile_publisher.publish(adcp); ros::spinOnce(); } - + // publish point cloud message if (pt_data_publisher.getNumSubscribers() > 0) { // prepare message header pt_msg.header.frame_id = pointcloud_frame; @@ -381,6 +387,7 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { ros::spinOnce(); } + // publish DVL contact range message if (rng_publisher.getNumSubscribers() > 0) { // prepare message header rng.header.frame_id = frame_name; @@ -413,7 +420,6 @@ void dsrosRosDvlSensor::UpdateChild(const gazebo::common::UpdateInfo &_info) { ros::spinOnce(); } - last_time = current_time; }