From c4505a406d7fe625aad3dc7f1112cf4380ff2be5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 15 Dec 2021 13:49:04 -0800 Subject: [PATCH] Update message documentation style Signed-off-by: Louise Poubel --- .../proto/lrauv_command.proto | 102 ++++++------- lrauv_ignition_plugins/proto/lrauv_init.proto | 24 ++- .../proto/lrauv_state.proto | 140 ++++++++++++------ .../src/TethysCommPlugin.cc | 8 +- .../src/TethysCommPlugin.hh | 2 - .../src/comms/CommsPacket.cc | 26 ++-- 6 files changed, 178 insertions(+), 124 deletions(-) diff --git a/lrauv_ignition_plugins/proto/lrauv_command.proto b/lrauv_ignition_plugins/proto/lrauv_command.proto index 9607fa9b..d688f416 100644 --- a/lrauv_ignition_plugins/proto/lrauv_command.proto +++ b/lrauv_ignition_plugins/proto/lrauv_command.proto @@ -37,58 +37,60 @@ message LRAUVCommand /// \brief Optional header data ignition.msgs.Header header = 1; - float propOmega_ = 2; // Angular velocity that the controller - // believes the propeller is currently at. - // Unit: rad / s. Positive values rotate - // the propeller clockwise when looking - // from the back, and propel the vehicle - // forward. - float rudderAngle_ = 3; // Angle that the controller believes the - // rudder is currently at. Unit: radians. - // Higher values have the vertical fins - // rotated more clockwise when looking - // from the top (i.e. to starboard) - float elevatorAngle_ = 4; // Angle that the controller believes the - // elevator is currently at. Unit: radians. - // Higher values have the horizontal fins - // rotated more counter-clockwise when - // looking from starboard, which tilts the - // nose downward when moving forward. - float massPosition_ = 5; // Position where the controller believes - // the mass shifter's joint is in. Unit: - // meters. Positive values have the battery - // forward, tilting the nose downward. - float buoyancyPosition_ = 6; // Volume that the controller believes the - // VBS currently has. Unit: cubic meters. - // Volumes higher than the neutral volume - // push the vehicle upward. - bool dropWeightState_ = 7; // Indicator "dropweight OK" - // 1 = in place, 0 = dropped - - float propOmegaAction_ = 8; // Target angular velocity for the - // propeller. Unit: rad / s. Positive - // values rotate the propeller clockwise - // when looking from the back, and propel - // the vehicle forward. - float rudderAngleAction_ = 9; // Target angle for rudder joint. Unit: - // radians. Higher values rotate the - // vertical fins clockwise when looking - // from the top (i.e. to starboard) - float elevatorAngleAction_ = 10; // Target angle for the elevator joint. - // Unit: radians. Higher values rotate - // vertical fins more counter-clockwise - // when looking from starboard, which tilts - // the nose downward when moving forward. - float massPositionAction_ = 11; // Target position for the battery's joint. - // Unit: meters. Positive values move the - // battery forward, tilting the nose - // downward. - float buoyancyAction_ = 12; // Target volume for the VBS (Variable - // Buoyancy System). Unit: cubic meters. - // Volumes higher than the neutral volume - // make the vehicle float. + /// \brief Angular velocity that the controller believes the propeller is currently at. + /// Unit: rad / s. Positive values rotate the propeller clockwise when looking from the + /// back, and propel the vehicle forward. + float propOmega_ = 2; + /// \brief Angle that the controller believes the rudder is currently at. Unit: radians. + /// Higher values have the vertical fins rotated more clockwise when looking from the + /// top (i.e. to starboard) + float rudderAngle_ = 3; + + /// \brief Angle that the controller believes the elevator is currently at. Unit: radians. + /// Higher values have the horizontal fins rotated more counter-clockwise when looking + /// from starboard, which tilts the nose downward when moving forward. + float elevatorAngle_ = 4; + + /// \brief Position where the controller believes the mass shifter's joint is in. Unit: + /// meters. Positive values have the battery forward, tilting the nose downward. + float massPosition_ = 5; + + /// \brief Volume that the controller believes the VBS currently has. Unit: cubic meters. + /// Volumes higher than the neutral volume push the vehicle upward. + float buoyancyPosition_ = 6; + + /// \brief Indicator "dropweight OK". 1 = in place, 0 = dropped + bool dropWeightState_ = 7; + + /// \brief Target angular velocity for the propeller. Unit: rad / s. Positive + /// values rotate the propeller clockwise when looking from the back, and propel + /// the vehicle forward. + float propOmegaAction_ = 8; + + /// \brief Target angle for rudder joint. Unit: radians. Higher values rotate the + /// vertical fins clockwise when looking from the top (i.e. to starboard) + float rudderAngleAction_ = 9; + + /// \brief Target angle for the elevator joint. Unit: radians. Higher values rotate + /// vertical fins more counter-clockwise when looking from starboard, which tilts + /// the nose downward when moving forward. + float elevatorAngleAction_ = 10; + + /// \brief Target position for the battery's joint. Unit: meters. Positive values move the + /// battery forward, tilting the nose downward. + float massPositionAction_ = 11; + + /// \brief Target volume for the VBS (Variable Buoyancy System). Unit: cubic meters. + /// Volumes higher than the neutral volume make the vehicle float. + float buoyancyAction_ = 12; + + /// \brief Not used float density_ = 13; + + /// \brief Not used float dt_ = 14; + + /// \brief Not used double time_ = 15; } diff --git a/lrauv_ignition_plugins/proto/lrauv_init.proto b/lrauv_ignition_plugins/proto/lrauv_init.proto index 90ad9cd0..cda429d1 100644 --- a/lrauv_ignition_plugins/proto/lrauv_init.proto +++ b/lrauv_ignition_plugins/proto/lrauv_init.proto @@ -41,13 +41,23 @@ message LRAUVInit /// \brief Unique ID identifying vehicle ignition.msgs.StringMsg id_ = 2; - // Initial Position Vector - double initLat_ = 3; // arcdeg // Initial latitude - double initLon_ = 4; // arcdeg // Initial longitude - double initZ_ = 5; // m // Initial depth - float initPitch_ = 6; // rad // Pitch wrto LV - float initRoll_ = 7; // rad // Roll wrto LV - float initHeading_ = 8; // rad // Yaw wrto LV + /// \brief Initial latitude in degrees. + double initLat_ = 3; + + /// \brief Initial longitude in degrees. + double initLon_ = 4; + + /// \brief Initial depth in meters. + double initZ_ = 5; + + /// \TODO(chapulina) + float initPitch_ = 6; + + /// \TODO(chapulina) + float initRoll_ = 7; + + /// \TODO(chapulina) + float initHeading_ = 8; // Add fields as needed } diff --git a/lrauv_ignition_plugins/proto/lrauv_state.proto b/lrauv_ignition_plugins/proto/lrauv_state.proto index e163620a..b0dd7455 100644 --- a/lrauv_ignition_plugins/proto/lrauv_state.proto +++ b/lrauv_ignition_plugins/proto/lrauv_state.proto @@ -33,66 +33,114 @@ import "ignition/msgs/header.proto"; import "ignition/msgs/vector3d.proto"; // Mirrors SimResultStruct -// Note coordinate frame convention for all pose fields: -// FSK (x fore or forward, y starboard or right, z keel or down) +// \TODO(chapulina) Update coordinate frame convention for all pose fields: message LRAUVState { /// \brief Optional header data + /// Stamped with simulation time. ignition.msgs.Header header = 1; + /// \brief Not populated int32 errorPad_ = 2; + + /// \brief Not populated int32 utmZone_ = 3; + + /// \brief Not populated int32 northernHemi_ = 4; - float propOmega_ = 5; // Current angular velocity of the - // propeller. Unit: rad / s. Positive - // values rotate the propeller - // clockwise when looking from the back, - // and propel the vehicle forward. - float propThrust_ = 6; // Not populated - float propTorque_ = 7; // Not populated - float rudderAngle_ = 8; // Angle that the rudder joint is - // currently at. Unit: radians. - // Higher values have the vertical fins - // rotated more clockwise when looking - // from the top (i.e. to starboard) + + /// \brief Current angular velocity of the propeller. Unit: rad / s. Positive + /// values rotate the propeller clockwise when looking from the back, + /// and propel the vehicle forward. + float propOmega_ = 5; + + /// \brief Not populated + float propThrust_ = 6; + + /// \brief Not populated + float propTorque_ = 7; + + /// \brief Angle that the rudder joint is currently at. Unit: radians. + /// Higher values have the vertical fins rotated more clockwise when looking + /// from the top (i.e. to starboard) + float rudderAngle_ = 8; + + /// \brief Angle that the elevator joint is currently at. Unit: radians. + /// Higher values have the horizontal fins rotated more counter-clockwise when + /// looking from starboard, which tilts the nose downward when moving forward. float elevatorAngle_ = 9; - float massPosition_ = 10; // Position of the battery's joint. - // Unit: meters. Positive values have - // the battery forward, tilting the - // nose downward. - float buoyancyPosition_ = 11; // Volume of the VBS. Unit: cubic - // meters. Volumes higher than the - // neutral volume push the vehicle - // upwards. + + /// \brief Position of the battery's joint. Unit: meters. Positive values have + /// the battery forward, tilting the nose downward. + float massPosition_ = 10; + + /// \brief Volume of the VBS. Unit: cubic meters. Volumes higher than the + /// neutral volume push the vehicle upwards. + float buoyancyPosition_ = 11; + + /// \brief Vertical position of the vehicle with respect to sea level. Higher + /// values are deeper, negative values are above water. Unit: meters. float depth_ = 12; - ignition.msgs.Vector3d rph_ = 13; // roll_, pitch_, heading_ in SimResultStruct (rad) + /// \brief Duplicate of posRPH_ + ignition.msgs.Vector3d rph_ = 13; + + /// \brief TODO(chapulina) + float speed_ = 14; - float speed_ = 14; // Magnitude of linear velocity in the - // world frame. Unit: m / s + /// \brief Latitude in degrees double latitudeDeg_ = 15; + + /// \brief Longitude in degrees double longitudeDeg_ = 16; - float netBuoy_ = 17; // Net buoyancy forces applied to the - // vehicle. Unit: Newtons. Currently - // not populated. - - ignition.msgs.Vector3d force_ = 18; // forceX_, forceY_, forceZ_ in SimResultStruct - ignition.msgs.Vector3d pos_ = 19; // posX_, posY_, posZ_ in SimResultStruct - ignition.msgs.Vector3d posRPH_ = 20; // posRoll_, posPitch_, posHeading_ in SimResultStruct - ignition.msgs.Vector3d posDot_ = 21; // posXDot_, posYDot_, posZDot_ in SimResultStruct - // Velocity wrt ground - ignition.msgs.Vector3d rateUVW_ = 22; // rateU_, rateV_, rateW_ in SimResultStruct - // Water velocity - ignition.msgs.Vector3d ratePQR_ = 23; // rateP_, rateQ_, rateR_ in SimResultStruct - // for roll, pitch, yaw rates, respectively - - float northCurrent_ = 24; // +Y velocity in m / s - float eastCurrent_ = 25; // +X velocity in m / s - float vertCurrent_ = 26; // +Z velocity in m / s + + /// \brief Net buoyancy forces applied to the vehicle. Unit: Newtons. Currently not populated. + float netBuoy_ = 17; + + /// \brief Not populated + ignition.msgs.Vector3d force_ = 18; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d pos_ = 19; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d posRPH_ = 20; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d posDot_ = 21; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d rateUVW_ = 22; + + /// \brief \TODO(chapulina) + ignition.msgs.Vector3d ratePQR_ = 23; + + /// \brief Northward sea water velocity collected from current sensor. Unit: m/s. + float northCurrent_ = 24; + + /// \brief Eastward sea water velocity collected from current sensor. Unit: m/s. + float eastCurrent_ = 25; + + /// \brief Not populated + float vertCurrent_ = 26; + + /// \brief Not populated float magneticVariation_ = 27; + + /// \brief Not populated float soundSpeed_ = 28; - float temperature_ = 29; // Celsius - float salinity_ = 30; // PSU - float density_ = 31; // Density of the surrounding water in Kg / m ^ 3 - repeated float values_ = 32; // Size 4 (0: chlorophyll in ug / L, 1: pressure in Pa) + + /// \brief Data collected from temperature sensor. Unit: Celsius + float temperature_ = 29; + + /// \brief Data collected from salinity sensor. Unit: PSU + float salinity_ = 30; + + /// \brief Density of the surrounding water in kg / m ^ 3 + float density_ = 31; + + /// \brief Size 4 + // 0: Data collected from Chlorophyll sensor. Unit: ug / L + // 1: Pressure calculated from current depth and latitude. Unit: Pa + repeated float values_ = 32; } diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc index 98a13098..b03beb26 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.cc +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.cc @@ -394,15 +394,10 @@ void TethysCommPlugin::SetupEntities( void TethysCommPlugin::CommandCallback( const lrauv_ignition_plugins::msgs::LRAUVCommand &_msg) { - // Lazy timestamp conversion just for printing - //if (std::chrono::seconds(int(floor(_msg.time_()))) - this->prevSubPrintTime - // > std::chrono::milliseconds(1000)) if (this->debugPrintout) { igndbg << "[" << this->ns << "] Received command: " << std::endl << _msg.DebugString() << std::endl; - - this->prevSubPrintTime = std::chrono::seconds(int(floor(_msg.time_()))); } // Rudder @@ -617,7 +612,8 @@ void TethysCommPlugin::PostUpdate( stateMsg.set_eastcurrent_(this->latestCurrent.X()); stateMsg.set_northcurrent_(this->latestCurrent.Y()); - stateMsg.set_vertcurrent_(this->latestCurrent.Z()); + // Not populating vertCurrent because we're not getting it from the science + // data this->statePub.Publish(stateMsg); diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.hh b/lrauv_ignition_plugins/src/TethysCommPlugin.hh index 87a76efc..1b18ae4c 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.hh +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.hh @@ -193,8 +193,6 @@ namespace tethys /// sanity check private: std::chrono::steady_clock::duration prevPubPrintTime = std::chrono::steady_clock::duration::zero(); - private: std::chrono::steady_clock::duration prevSubPrintTime = - std::chrono::steady_clock::duration::zero(); /// Transport node for message passing private: ignition::transport::Node node; diff --git a/lrauv_ignition_plugins/src/comms/CommsPacket.cc b/lrauv_ignition_plugins/src/comms/CommsPacket.cc index 6581a0f9..8b27f84a 100644 --- a/lrauv_ignition_plugins/src/comms/CommsPacket.cc +++ b/lrauv_ignition_plugins/src/comms/CommsPacket.cc @@ -85,7 +85,7 @@ std::string CommsPacket::Data() const } ////////////////////////////////////////////////// -lrauv_ignition_plugins::msgs::LRAUVAcousticMessage +lrauv_ignition_plugins::msgs::LRAUVAcousticMessage CommsPacket::ToExternalMsg() const { lrauv_ignition_plugins::msgs::LRAUVAcousticMessage msg; @@ -93,7 +93,7 @@ lrauv_ignition_plugins::msgs::LRAUVAcousticMessage msg.set_from(this->dataPtr->from); msg.set_to(this->dataPtr->to); - using MsgType + using MsgType = lrauv_ignition_plugins::msgs::LRAUVAcousticMessage::MessageType; if (this->Type() == CommsPacket::MsgType::RANGE_REQUEST) @@ -128,7 +128,7 @@ lrauv_ignition_plugins::msgs::LRAUVInternalComms msg.set_allocated_position(vector); ignition::msgs::Time* time = new ignition::msgs::Time; - auto sec = + auto sec = std::chrono::duration_cast( this->dataPtr->timeOfTx.time_since_epoch() ); @@ -142,7 +142,7 @@ lrauv_ignition_plugins::msgs::LRAUVInternalComms msg.set_allocated_header(header); - using MsgType + using MsgType = lrauv_ignition_plugins::msgs::LRAUVInternalComms::MessageType; if (this->Type() == CommsPacket::MsgType::RANGE_REQUEST) @@ -175,22 +175,22 @@ CommsPacket CommsPacket::make( packet.dataPtr->position = position; packet.dataPtr->timeOfTx = timeOfTx; - using MsgType + using MsgType = lrauv_ignition_plugins::msgs::LRAUVAcousticMessage::MessageType; - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVAcousticMessage_MessageType_RangeRequest) { packet.dataPtr->type = CommsPacket::MsgType::RANGE_REQUEST; } - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVAcousticMessage_MessageType_RangeResponse) { packet.dataPtr->type = CommsPacket::MsgType::RANGE_RESPONSE; } - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVAcousticMessage_MessageType_Other) { packet.dataPtr->type = CommsPacket::MsgType::DATA; @@ -207,7 +207,7 @@ CommsPacket CommsPacket::make( packet.dataPtr->to = datapayload.to(); packet.dataPtr->from = datapayload.from(); packet.dataPtr->data = datapayload.data(); - + ignition::math::Vector3d position; position.X(datapayload.position().x()); position.Y(datapayload.position().y()); @@ -219,22 +219,22 @@ CommsPacket CommsPacket::make( std::chrono::steady_clock::time_point timeOfTx(dur); packet.dataPtr->timeOfTx = timeOfTx; - using MsgType + using MsgType = lrauv_ignition_plugins::msgs::LRAUVInternalComms::MessageType; - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVInternalComms_MessageType_RangeRequest) { packet.dataPtr->type = CommsPacket::MsgType::RANGE_REQUEST; } - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVInternalComms_MessageType_RangeResponse) { packet.dataPtr->type = CommsPacket::MsgType::RANGE_RESPONSE; } - if (datapayload.type() + if (datapayload.type() == MsgType::LRAUVInternalComms_MessageType_Other) { packet.dataPtr->type = CommsPacket::MsgType::DATA;