Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update message documentation style #119

Merged
merged 1 commit into from
Dec 15, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
102 changes: 52 additions & 50 deletions lrauv_ignition_plugins/proto/lrauv_command.proto
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
24 changes: 17 additions & 7 deletions lrauv_ignition_plugins/proto/lrauv_init.proto
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
140 changes: 94 additions & 46 deletions lrauv_ignition_plugins/proto/lrauv_state.proto
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
/// \brief Vertical position of the vehicle with respect to sea level. Higher
/// \brief Vertical position of the vehicle with respect to mean sea level. Higher

Sea level is ambiguous with tides. There's MSL or there's depth to current surface, or several other semi-obscure options. This could be alternatively switched to a TODO.

/// 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)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
/// \brief TODO(chapulina)
/// \brief World frame velocity in m/s TODO(chapulina) check axes etc

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)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
/// \brief \TODO(chapulina)
/// \brief Velocity wrt to ground \TODO(chapulina) check axes

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ignition.msgs.Vector3d posDot_ = 21;

/// \brief \TODO(chapulina)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
/// \brief \TODO(chapulina)
/// \brief Water velocity \TODO(chapulina) check axes etc

ignition.msgs.Vector3d rateUVW_ = 22;

/// \brief \TODO(chapulina)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
/// \brief \TODO(chapulina)
/// \brief Roll pitch and yaw rates \TODO(chapulina) check axes etc.

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
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
/// \brief Data collected from salinity sensor. Unit: PSU
/// \brief Data collected from salinity sensor. Unit: PSU (Practical Salinity Unit or g/kg)

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;
}
8 changes: 2 additions & 6 deletions lrauv_ignition_plugins/src/TethysCommPlugin.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);

Expand Down
2 changes: 0 additions & 2 deletions lrauv_ignition_plugins/src/TethysCommPlugin.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading