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

Added integration test using multiple lrauv vehicles #243

Merged
merged 19 commits into from
Sep 9, 2022
Merged
Show file tree
Hide file tree
Changes from 14 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
10 changes: 3 additions & 7 deletions lrauv_description/models/tethys_equipped/model.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -374,14 +374,10 @@
</plugin>

<plugin
filename="AcousticCommsPlugin"
name="tethys::AcousticCommsPlugin">
filename="gz-sim-comms-endpoint-system"
name="gz::sim::systems::CommsEndpoint">
<address>1</address>
<model_plugin_file>simple_acoustic_model</model_plugin_file>
<model_name>tethys::SimpleAcousticModel</model_name>
<link_name>acoustic_transponder</link_name>
<max_range>2500</max_range>
<speed_of_sound>1500</speed_of_sound>
<topic>1/rx</topic>
</plugin>

<plugin
Expand Down
6 changes: 0 additions & 6 deletions lrauv_ignition_plugins/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -151,12 +151,6 @@ endfunction()

add_subdirectory(src/comms/)

add_lrauv_plugin(AcousticCommsPlugin
PROTO
lrauv_acoustic_message
lrauv_internal_comms
PRIVATE_LINK_LIBS
acoustic_comms_support)
add_lrauv_plugin(ControlPanelPlugin GUI
PROTO
lrauv_command
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +25,23 @@

#include <functional>

#include <gz/msgs.hh>
#include <gz/transport/Node.hh>

#include "CommsPacket.hh"
#include "TopicDefinitions.hh"
/* #include "CommsPacket.hh" */

#include "lrauv_ignition_plugins/lrauv_acoustic_message.pb.h"

using LRAUVAcousticMessage =
lrauv_ignition_plugins::msgs::LRAUVAcousticMessage;
using MessageType = LRAUVAcousticMessage::MessageType;
static constexpr auto LRAUVAcousticMessageType =
MessageType::LRAUVAcousticMessage_MessageType_Other;
auto RangeRequest =
MessageType::LRAUVAcousticMessage_MessageType_RangeRequest;
auto RangeResponse =
MessageType::LRAUVAcousticMessage_MessageType_RangeResponse;
adityapande-1995 marked this conversation as resolved.
Show resolved Hide resolved

namespace tethys
{
//////////////////////////////////////////////////
Expand All @@ -47,16 +57,13 @@ using Callback = std::function<void(const CommsMsg&)>;
/// \param[in] _commsPrefix - Optional. Use if your public interface to acoustic
/// comms is different.
public: CommsClient(uint32_t _address,
Callback _callback,
std::string _commsPrefix=EXTERNAL_COMMS_BUS) :
Callback _callback) :
address(_address), callback(std::move(_callback))
{
this->transmitter =
this->node.Advertise<CommsMsg>(
_commsPrefix + "/" + std::to_string(address) + "/tx");
this->node.Advertise<gz::msgs::Dataframe>("/broker/msgs");

this->node.Subscribe(
_commsPrefix + "/" + std::to_string(address) + "/rx",
this->node.Subscribe(std::to_string(address) + "/rx",
&CommsClient::ReceivedPacket,
this
);
Expand All @@ -67,15 +74,58 @@ public: CommsClient(uint32_t _address,
/// \param[in] _msg - Comms message to be sent
public: void SendPacket(const CommsMsg& _msg)
{
this->transmitter.Publish(_msg);
// Populate a Dataframe msg based on the
// CommsMsg content.
gz::msgs::Dataframe message;
message.set_src_address(
std::to_string(_msg.from()));
message.set_dst_address(
std::to_string(_msg.to()));
message.set_data(_msg.data());

// Set message type
auto *frame = message.mutable_header()->add_data();
frame->set_key("msg_type");
if (_msg.type() == LRAUVAcousticMessageType)
frame->add_value("LRAUVAcousticMessageType");
else if (_msg.type() == RangeRequest)
frame->add_value("RangeRequest");
else if (_msg.type() == RangeResponse)
frame->add_value("RangeResponse");
adityapande-1995 marked this conversation as resolved.
Show resolved Hide resolved

// Publish the Dataframe message
this->transmitter.Publish(message);
}

//////////////////////////////////////////////////
/// \brief Callback handler
/// \param[in] _msg - message received
private: void ReceivedPacket(const CommsMsg& _msg)
private: void ReceivedPacket(const gz::msgs::Dataframe& _msg)
{
this->callback(_msg);
// Construct a CommsMsg from the received Dataframe msg.
CommsMsg message;
message.set_from(
std::stoi(_msg.src_address()));
message.set_to(
std::stoi(_msg.dst_address()));
message.set_data(_msg.data());

// Set message type.
for (int i = 0; i < _msg.header().data_size(); i++)
{
if (_msg.header().data(i).key() == "msg_type")
{
auto type = _msg.header().data(i).value().Get(0);
if (type == "LRAUVAcousticMessageType")
message.set_type(LRAUVAcousticMessageType);
else if (type == "RangeResponse")
message.set_type(RangeResponse);
else if (type == "RangeRequest")
message.set_type(RangeRequest);
}
}

this->callback(message);
}

/// \brief Address which the client binds to
Expand Down

This file was deleted.

This file was deleted.

This file was deleted.

Loading