diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt index 0608a66f..5fb7e36d 100644 --- a/lrauv_ignition_plugins/CMakeLists.txt +++ b/lrauv_ignition_plugins/CMakeLists.txt @@ -207,6 +207,8 @@ install( foreach(EXAMPLE example_buoyancy example_controller + example_comms_client + example_comms_echo example_dvl_velocity example_elevator example_mass_shifter @@ -220,6 +222,7 @@ foreach(EXAMPLE add_executable(${EXAMPLE_EXEC} example/${EXAMPLE}.cc) set_property(TARGET ${EXAMPLE_EXEC} PROPERTY CXX_STANDARD 17) + target_include_directories(${EXAMPLE_EXEC} PUBLIC include) target_link_libraries(${EXAMPLE_EXEC} PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} dvl_velocity_tracking lrauv_acoustic_message lrauv_command lrauv_init diff --git a/lrauv_ignition_plugins/example/example_comms_client.cc b/lrauv_ignition_plugins/example/example_comms_client.cc new file mode 100644 index 00000000..6f8f09f5 --- /dev/null +++ b/lrauv_ignition_plugins/example/example_comms_client.cc @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +/** + * This is a small example of usage of CommsClient to send and receive data + * packets using the acoustic subsystem. For more info: + * https://github.com/osrf/lrauv/wiki/Examples-and-tutorials#using-commsclient + * + * Usage: + * $ ./LRAUV_example_comms_client + */ + +#include + +using namespace tethys; +using AcousticMsg = lrauv_ignition_plugins::msgs::LRAUVAcousticMessage; + +using namespace std::literals::chrono_literals; + +int main(int _argc, char **_argv) +{ + // For sending data + // Bind sender to address 1 + constexpr int senderAddress = 1; + CommsClient sender(senderAddress, [](const auto){}); + + bool messageReceived = false; + std::mutex messageArrivalMutex; + std::condition_variable messageArrival; + + // For receiving data + // Bind receiver to address 2 + constexpr int receiverAddress = 2; + CommsClient receiver(receiverAddress, [&](const auto msg) + { + // Your callback function + // To get who the message is from + std::cout << "From: " << msg.from() << std::endl; + // To get the data call + std::cout << "Data: " << msg.data() << std::endl; + + std::lock_guard lock(messageArrivalMutex); + messageReceived = true; + messageArrival.notify_all(); + }); + + AcousticMsg msg; + // Who to send to + msg.set_to(receiverAddress); + // From who + msg.set_from(senderAddress); + // Message type + msg.set_type(AcousticMsg::MessageType::LRAUVAcousticMessage_MessageType_Other); + // Adding the data + msg.set_data("test_message"); + sender.SendPacket(msg); + + using namespace std::literals::chrono_literals; + std::unique_lock lock(messageArrivalMutex); + if (!messageArrival.wait_for( + lock, 5s, [&] { return messageReceived; })) + { + std::cout << "5s timeout, message not received." << std::endl; + } +} diff --git a/lrauv_ignition_plugins/example/example_comms_echo.cc b/lrauv_ignition_plugins/example/example_comms_echo.cc new file mode 100644 index 00000000..158fb319 --- /dev/null +++ b/lrauv_ignition_plugins/example/example_comms_echo.cc @@ -0,0 +1,95 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Development of this module has been funded by the Monterey Bay Aquarium + * Research Institute (MBARI) and the David and Lucile Packard Foundation + */ + +/** + * This is a small example of usage of gazebo transport to send and receive + * data packets using the acoustic subsystem. For more info: + * https://github.com/osrf/lrauv/wiki/Examples-and-tutorials#direct-method + * + * Usage: + * $ ./LRAUV_example_comms_echo + */ + +#include +#include +#include +#include "lrauv_ignition_plugins/lrauv_acoustic_message.pb.h" + +int address = 1; +gz::transport::Node::Publisher transmitter; +using AcousticMsg = lrauv_ignition_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) +{ + std::cout << _msg.from() << ": " << _msg.data(); + + lrauv_ignition_plugins::msgs::LRAUVAcousticMessage returnMsg; + // Who to send to + returnMsg.set_to(_msg.from()); + + // From who + returnMsg.set_from(address); + + // `LRAUVAcousticMessage_MessageType_Other` means its a data packet + returnMsg.set_type(AcousticMsg::MessageType::LRAUVAcousticMessage_MessageType_Other); + + // The data + returnMsg.set_data(_msg.data()); + + // Send the data + transmitter.Publish(returnMsg); +} + +int main(int argc, char** argv) +{ + gz::transport::Node node; + + if (argc > 1) + { + std::istringstream ss(argv[1]); + if (!(ss >> address)) + { + std::cerr << "Invalid number: " << argv[1] + << ". Using default value 1 instead." << std::endl; + } + else + { + if (!ss.eof()) + { + std::cerr << "Trailing characters after number: " << argv[1] + << ". Using default value 1 instead." << std::endl; + } + } + } + + transmitter = node.Advertise( + "/comms/external/" + std::to_string(address) + "/tx"); + + node.Subscribe( + "/comms/external/" + std::to_string(address) + "/rx", + cb + ); + + gz::transport::waitForShutdown(); +}