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

Ignition to Gazebo migration #237

Merged
merged 12 commits into from
Sep 14, 2022
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
12 changes: 6 additions & 6 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ Stability is not guaranteed.

Source files, models, and plugins relevant to a general audience are upstreamed
on an irregular basis to Ignition libraries, the top-level library being
[ign-gazebo](https:/ignitionrobotics/ign-gazebo/).
[ign-gazebo](https:/gazebosim/ign-gazebo/).
Upstreamed files may eventually be removed from this repository.

Standalone, this repository contains the environment and plugins necessary to
Expand Down Expand Up @@ -39,7 +39,7 @@ docker/join.sh mbari_lrauv
## To build

To run the code in this repository natively without Docker, make sure you have
[Ignition Garden](https://ignitionrobotics.org/docs/garden) and
[Ignition Garden](https://gazebosim.org/docs/garden) and
[colcon](https://colcon.readthedocs.io/en/released/), on Ubuntu Focal or higher.

Install dependencies
Expand All @@ -58,7 +58,7 @@ colcon build --cmake-args "-DBUILD_TESTING=ON"
```

> You can pass `--cmake-args ' -DENABLE_PROFILER=1'` to use the profiler.
> See more on [this tutorial](https://ignitionrobotics.org/api/common/4.4/profiler.html)
> See more on [this tutorial](https://gazebosim.org/api/common/4.4/profiler.html)

## To test simulation in Ignition standalone (without MBARI integration)

Expand Down Expand Up @@ -492,11 +492,11 @@ for example:

## Levels

Some worlds support [levels](https://ignitionrobotics.org/api/gazebo/6.7/levels.html).
Some worlds support [levels](https://gazebosim.org/api/gazebo/6.7/levels.html).
Levels are turned off by default, which means that all heightmap tiles will
be loaded at all times. When levels are enabled, only the tiles containing
vehicles (performers) spawned with `WorldCommPlugin` with an
[lrauv_init](https:/osrf/lrauv/blob/main/lrauv_ignition_plugins/proto/lrauv_init.proto)
[lrauv_init](https:/osrf/lrauv/blob/main/lrauv_gazebo_plugins/proto/lrauv_init.proto)
message will be loaded.

For example, loading without levels:
Expand Down Expand Up @@ -529,7 +529,7 @@ On the MBARI Main Vehicle Application side, all values during the run are
stored to disk.
They can be retrieved after the run and plotted for debugging purposes.

See [`lrauv_ignition_plugins/plots/README.md`](https:/osrf/lrauv/blob/main/lrauv_ignition_plugins/plots/README.md)
See [`lrauv_gazebo_plugins/plots/README.md`](https:/osrf/lrauv/blob/main/lrauv_gazebo_plugins/plots/README.md)
for instructions to unserialize and scripts for plotting.

## Science data
Expand Down
4 changes: 2 additions & 2 deletions docker/debug_integration/mux.yml
Original file line number Diff line number Diff line change
Expand Up @@ -17,5 +17,5 @@ windows:
- logging:
panes:
- read -n 1 -p "Press any key to export logs:" mainmenuinput;
sudo /home/developer/lrauv_ws/src/lrauv/lrauv_ignition_plugins/plots/unserialize_for_plotting.sh;
sudo cp -r /home/developer/lrauv_ws/src/lrauv/lrauv_ignition_plugins/plots/missions/tmp /results/
sudo /home/developer/lrauv_ws/src/lrauv/lrauv_gazebo_plugins/plots/unserialize_for_plotting.sh;
sudo cp -r /home/developer/lrauv_ws/src/lrauv/lrauv_gazebo_plugins/plots/missions/tmp /results/
30 changes: 15 additions & 15 deletions lrauv_description/models/tethys_equipped/model.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -19,64 +19,64 @@
action="add"
name="salinity_sensor"
type="custom"
ignition:type="salinity">
gz:type="salinity">
<always_on>1</always_on>
<update_rate>2</update_rate>
<topic>/model/tethys/salinity</topic>
<ignition:salinity>
<gz:salinity>
<noise type="gaussian">
<mean>0.00001</mean>
<stddev>0.00001</stddev>
</noise>
</ignition:salinity>
</gz:salinity>
</sensor>
<sensor
element_id="base_link"
action="add"
name="temperature_sensor"
type="custom"
ignition:type="temperature">
gz:type="temperature">
<always_on>1</always_on>
<update_rate>2</update_rate>
<topic>/model/tethys/temperature</topic>
<ignition:temperature>
<gz:temperature>
<noise type="gaussian">
<mean>0.00001</mean>
<stddev>0.00001</stddev>
</noise>
</ignition:temperature>
</gz:temperature>
</sensor>
<sensor
element_id="base_link"
action="add"
name="chlorophyll_sensor"
type="custom"
ignition:type="chlorophyll">
gz:type="chlorophyll">
<always_on>1</always_on>
<update_rate>2</update_rate>
<topic>/model/tethys/chlorophyll</topic>
<ignition:chlorophyll>
<gz:chlorophyll>
<noise type="gaussian">
<mean>0.00001</mean>
<stddev>0.00001</stddev>
</noise>
</ignition:chlorophyll>
</gz:chlorophyll>
</sensor>
<sensor
element_id="base_link"
action="add"
name="current_sensor"
type="custom"
ignition:type="current">
gz:type="current">
<always_on>1</always_on>
<update_rate>2</update_rate>
<topic>/model/tethys/current</topic>
<ignition:current>
<gz:current>
<noise type="gaussian">
<mean>0.00001</mean>
<stddev>0.00001</stddev>
</noise>
</ignition:current>
</gz:current>
</sensor>
<!--
Teledyne Pathfinder DVL
Expand All @@ -87,13 +87,13 @@
<sensor
element_id="base_link" action="add"
name="teledyne_pathfinder_dvl"
type="custom" ignition:type="dvl">
type="custom" gz:type="dvl">
<!-- Account for DVL mounting position and base link rotation -->
<pose degrees="true">-0.60 0 -0.16 0 0 180</pose>
<always_on>1</always_on>
<update_rate>1</update_rate>
<topic>/tethys/dvl/velocity</topic>
<ignition:dvl>
<gz:dvl>
<arrangement degrees="true">
<beam id="1">
<aperture>2.1</aperture>
Expand Down Expand Up @@ -128,7 +128,7 @@
<maximum_range>80.</maximum_range>
<!-- ENU to SFM -->
<reference_frame>0 0 0 0 0 -1.570796</reference_frame>
</ignition:dvl>
</gz:dvl>
</sensor>
<!--
Sparton AHRS-M2 arrangement of IMU + Magnetometer
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@

cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)

project(lrauv_ignition_plugins)
project(lrauv_gazebo_plugins)

include(CTest)

Expand Down
6 changes: 6 additions & 0 deletions lrauv_gazebo_plugins/colcon.pkg
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
{
"hooks": [
"share/lrauv_gazebo_plugins/hooks/hook.dsv",
"share/lrauv_gazebo_plugins/hooks/hook.sh"
]
}
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@

#include <gz/msgs.hh>
#include <gz/transport.hh>
#include "lrauv_ignition_plugins/lrauv_command.pb.h"
#include "lrauv_gazebo_plugins/lrauv_command.pb.h"

int main(int _argc, char **_argv)
{
Expand All @@ -54,15 +54,15 @@ int main(int _argc, char **_argv)
gz::transport::Node node;
auto commandTopic = "/" + vehicleName + "/command_topic";
auto commandPub =
node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(commandTopic);
node.Advertise<lrauv_gazebo_plugins::msgs::LRAUVCommand>(commandTopic);

while (!commandPub.HasConnections())
{
std::cout << "Command publisher waiting for connections..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}

lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand cmdMsg;
cmdMsg.set_buoyancyaction_(volume);

// Don't release drop-weight
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,10 +29,10 @@
* $ ./LRAUV_example_comms_client
*/

#include <lrauv_ignition_plugins/comms/CommsClient.hh>
#include <lrauv_gazebo_plugins/comms/CommsClient.hh>

using namespace tethys;
using AcousticMsg = lrauv_ignition_plugins::msgs::LRAUVAcousticMessage;
using AcousticMsg = lrauv_gazebo_plugins::msgs::LRAUVAcousticMessage;

using namespace std::literals::chrono_literals;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,19 +32,19 @@
#include <sstream>
#include <unistd.h>
#include <gz/transport/Node.hh>
#include "lrauv_ignition_plugins/lrauv_acoustic_message.pb.h"
#include "lrauv_gazebo_plugins/lrauv_acoustic_message.pb.h"

int address = 1;
gz::transport::Node::Publisher transmitter;
using AcousticMsg = lrauv_ignition_plugins::msgs::LRAUVAcousticMessage;
using AcousticMsg = lrauv_gazebo_plugins::msgs::LRAUVAcousticMessage;

//////////////////////////////////////////////////
/// \brief Function called each time a message is recieved by the comms subsystem.
void cb(const lrauv_ignition_plugins::msgs::LRAUVAcousticMessage &_msg)
void cb(const AcousticMsg &_msg)
{
std::cout << _msg.from() << ": " << _msg.data();

lrauv_ignition_plugins::msgs::LRAUVAcousticMessage returnMsg;
AcousticMsg returnMsg;
// Who to send to
returnMsg.set_to(_msg.from());

Expand Down Expand Up @@ -83,7 +83,7 @@ int main(int argc, char** argv)
}
}

transmitter = node.Advertise<lrauv_ignition_plugins::msgs::LRAUVAcousticMessage>(
transmitter = node.Advertise<AcousticMsg>(
"/comms/external/" + std::to_string(address) + "/tx");

node.Subscribe(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@

#include <gz/msgs.hh>
#include <gz/transport.hh>
#include "lrauv_ignition_plugins/lrauv_command.pb.h"
#include "lrauv_gazebo_plugins/lrauv_command.pb.h"

int main(int _argc, char **_argv)
{
Expand All @@ -47,15 +47,15 @@ int main(int _argc, char **_argv)
gz::transport::Node node;
auto commandTopic = "/" + vehicleName + "/command_topic";
auto commandPub =
node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(commandTopic);
node.Advertise<lrauv_gazebo_plugins::msgs::LRAUVCommand>(commandTopic);

double angle = 0.17;

// Wiggle rudder
for (int i = 0; i < 10; i++)
{
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
lrauv_ignition_plugins::msgs::LRAUVCommand rudderMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand rudderMsg;
angle *= -1;
rudderMsg.set_buoyancyaction_(0.0005); // Keep it stable
rudderMsg.set_dropweightstate_(1);
Expand All @@ -65,7 +65,7 @@ int main(int _argc, char **_argv)
}

std::this_thread::sleep_for(std::chrono::milliseconds(1000));
lrauv_ignition_plugins::msgs::LRAUVCommand rudderStopMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand rudderStopMsg;
rudderStopMsg.set_buoyancyaction_(0.0005);
rudderStopMsg.set_rudderangleaction_(0);
rudderStopMsg.set_dropweightstate_(1);
Expand All @@ -76,7 +76,7 @@ int main(int _argc, char **_argv)
for (int i = 0; i < 10; i++)
{
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
lrauv_ignition_plugins::msgs::LRAUVCommand elevatorMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand elevatorMsg;
angle *= -1;
elevatorMsg.set_buoyancyaction_(0.0005);
elevatorMsg.set_elevatorangleaction_(angle);
Expand All @@ -86,23 +86,23 @@ int main(int _argc, char **_argv)
}

std::this_thread::sleep_for(std::chrono::milliseconds(1000));
lrauv_ignition_plugins::msgs::LRAUVCommand elevatorStopMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand elevatorStopMsg;
elevatorStopMsg.set_buoyancyaction_(0.0005);
elevatorStopMsg.set_elevatorangleaction_(0);
elevatorStopMsg.set_dropweightstate_(1);
commandPub.Publish(elevatorStopMsg);
std::cout << "moving elevator to " << angle << "\n";

// Charge forward
lrauv_ignition_plugins::msgs::LRAUVCommand thrustMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand thrustMsg;
thrustMsg.set_buoyancyaction_(0.0005);
thrustMsg.set_propomegaaction_(30);
thrustMsg.set_dropweightstate_(1);
commandPub.Publish(thrustMsg);
std::cout << "charging forward!\n";

std::this_thread::sleep_for(std::chrono::milliseconds(10000));
lrauv_ignition_plugins::msgs::LRAUVCommand stopMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand stopMsg;
stopMsg.set_buoyancyaction_(0.0005);
stopMsg.set_propomegaaction_(0);
stopMsg.set_dropweightstate_(1);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
/**
* Subscribe to DVL velocity tracking estimates.
*
* An ``ign topic -e`` command equivalent that can subscribe
* An ``gz topic -e`` command equivalent that can subscribe
* and print DVL velocity tracking messages to console.
*
* Usage:
Expand All @@ -36,9 +36,9 @@
#include <gz/msgs.hh>
#include <gz/transport.hh>

#include "lrauv_ignition_plugins/dvl_velocity_tracking.pb.h"
#include "lrauv_gazebo_plugins/dvl_velocity_tracking.pb.h"

void callback(const lrauv_ignition_plugins::msgs::DVLVelocityTracking &_msg)
void callback(const lrauv_gazebo_plugins::msgs::DVLVelocityTracking &_msg)
{
std::cout << "---" << std::endl << _msg.DebugString();
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

#include <gz/msgs.hh>
#include <gz/transport.hh>
#include "lrauv_ignition_plugins/lrauv_command.pb.h"
#include "lrauv_gazebo_plugins/lrauv_command.pb.h"

int main(int _argc, char **_argv)
{
Expand All @@ -49,15 +49,15 @@ int main(int _argc, char **_argv)
gz::transport::Node node;
auto commandTopic = "/" + vehicleName + "/command_topic";
auto commandPub =
node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(commandTopic);
node.Advertise<lrauv_gazebo_plugins::msgs::LRAUVCommand>(commandTopic);

while (!commandPub.HasConnections())
{
std::cout << "Command publisher waiting for connections..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}

lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand cmdMsg;
cmdMsg.set_elevatorangleaction_(angle);

// Keep it stable
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@

#include <gz/msgs.hh>
#include <gz/transport.hh>
#include "lrauv_ignition_plugins/lrauv_command.pb.h"
#include "lrauv_gazebo_plugins/lrauv_command.pb.h"

int main(int _argc, char **_argv)
{
Expand All @@ -55,15 +55,15 @@ int main(int _argc, char **_argv)
gz::transport::Node node;
auto commandTopic = "/" + vehicleName + "/command_topic";
auto commandPub =
node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(commandTopic);
node.Advertise<lrauv_gazebo_plugins::msgs::LRAUVCommand>(commandTopic);

while (!commandPub.HasConnections())
{
std::cout << "Command publisher waiting for connections..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}

lrauv_ignition_plugins::msgs::LRAUVCommand batteryMsg;
lrauv_gazebo_plugins::msgs::LRAUVCommand batteryMsg;
batteryMsg.set_masspositionaction_(pos);

// Keep it stable
Expand Down
Loading