diff --git a/sw/CycloneDDS/Dockerfile/Dockerfile b/sw/CycloneDDS/Dockerfile/Dockerfile new file mode 100644 index 0000000..9aa2f8b --- /dev/null +++ b/sw/CycloneDDS/Dockerfile/Dockerfile @@ -0,0 +1,41 @@ +FROM debian:bullseye + +# Install Build Environment +RUN set -ex \ + && apt-get update \ + && apt-get install --yes --no-install-recommends \ + cmake \ + bison \ + g++ \ + make \ + openssl \ + && apt-get clean \ + && rm -rf /var/lib/apt/lists/* + +# Build and Install CycloneDDS +RUN set -ex \ + && apt-get update \ + && apt-get install --yes --no-install-recommends \ + ca-certificates \ + git \ + && cd /tmp \ + && git clone https://github.com/eclipse-cyclonedds/cyclonedds.git \ + && cd cyclonedds \ + && git checkout 0.8.0 \ + && mkdir build \ + && cd build \ + && cmake -DCMAKE_INSTALL_PREFIX=/opt/CycloneDDS -DBUILD_IDLC=ON .. \ + && cmake --build . \ + && cmake --build . --target install \ + && cd /tmp \ + && rm -rf * \ + && apt-get purge --yes \ + ca-certificates \ + git \ + && apt-get autoremove --yes \ + && apt-get clean \ + && rm -rf /var/lib/apt/lists/* + +COPY entrypoint.sh /entrypoint.sh +ENTRYPOINT ["/bin/bash", "/entrypoint.sh"] +CMD ["bash"] diff --git a/sw/CycloneDDS/Dockerfile/build.sh b/sw/CycloneDDS/Dockerfile/build.sh new file mode 100644 index 0000000..1f7c923 --- /dev/null +++ b/sw/CycloneDDS/Dockerfile/build.sh @@ -0,0 +1,3 @@ +#!/bin/sh + +docker build --progress=plain --tag cyclonedds:0.8.0 ./ diff --git a/sw/CycloneDDS/Dockerfile/entrypoint.sh b/sw/CycloneDDS/Dockerfile/entrypoint.sh new file mode 100644 index 0000000..ad07475 --- /dev/null +++ b/sw/CycloneDDS/Dockerfile/entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +export PATH=/opt/CycloneDDS/bin:$PATH +export LD_LIBRARY_PATH=/opt/CycloneDDS/lib:$LD_LIBRARY_PATH + +exec $1 diff --git a/sw/CycloneDDS/README.txt b/sw/CycloneDDS/README.txt new file mode 100644 index 0000000..0272f0f --- /dev/null +++ b/sw/CycloneDDS/README.txt @@ -0,0 +1,20 @@ +Tested with Docker Image build from ./Dockerfile/Dockerfile + + + +docker run --rm -it -v /home/john64/Desktop/rtps-fpga/sw/CycloneDDS/src:/test cyclonedds:0.8.0 bash +$ cd /test/helloworld +$ mkdir build +$ cd build +$ cmake .. +$ cmake --build . +$ ./HelloworldSubscriber +$ ./HelloworldPublisher + +docker run --rm -it --net host -v /home/john64/Desktop/rtps-fpga/sw/CycloneDDS/src:/test cyclonedds:0.8.0 bash +$ cd /test/Loopback +$ mkdir build +$ cd build +$ cmake .. +$ cmake --build . +$ ./Loopback diff --git a/sw/CycloneDDS/src/Loopback/.gitignore b/sw/CycloneDDS/src/Loopback/.gitignore new file mode 100644 index 0000000..f65519e --- /dev/null +++ b/sw/CycloneDDS/src/Loopback/.gitignore @@ -0,0 +1 @@ +build/** diff --git a/sw/CycloneDDS/src/Loopback/CMakeLists.txt b/sw/CycloneDDS/src/Loopback/CMakeLists.txt new file mode 100644 index 0000000..183e4e5 --- /dev/null +++ b/sw/CycloneDDS/src/Loopback/CMakeLists.txt @@ -0,0 +1,31 @@ +# +# Copyright(c) 2021 ADLINK Technology Limited and others +# +# This program and the accompanying materials are made available under the +# terms of the Eclipse Public License v. 2.0 which is available at +# http://www.eclipse.org/legal/epl-2.0, or the Eclipse Distribution License +# v. 1.0 which is available at +# http://www.eclipse.org/org/documents/edl-v10.php. +# +# SPDX-License-Identifier: EPL-2.0 OR BSD-3-Clause +# +project(Loopback LANGUAGES C) +cmake_minimum_required(VERSION 3.5) + +if(NOT TARGET CycloneDDS::ddsc) + # Find the CycloneDDS package. + find_package(CycloneDDS REQUIRED) +endif() + +# This is a convenience function, provided by the CycloneDDS package, +# that will supply a library target related the the given idl file. +# In short, it takes the idl file, generates the source files with +# the proper data types and compiles them into a library. +idlc_generate(TARGET Type1_lib FILES "Type1.idl") + +# Both executables have only one related source file. +add_executable(Loopback loopback.c) + +# Both executables need to be linked to the idl data type library and +# the ddsc API library. +target_link_libraries(Loopback Type1_lib CycloneDDS::ddsc) diff --git a/sw/CycloneDDS/src/Loopback/Type1.idl b/sw/CycloneDDS/src/Loopback/Type1.idl new file mode 100644 index 0000000..1300c2c --- /dev/null +++ b/sw/CycloneDDS/src/Loopback/Type1.idl @@ -0,0 +1,5 @@ +@extensibility(FINAL) +struct Type1 { + @key long id; + long a; +}; \ No newline at end of file diff --git a/sw/CycloneDDS/src/Loopback/loopback.c b/sw/CycloneDDS/src/Loopback/loopback.c new file mode 100644 index 0000000..c6b8a29 --- /dev/null +++ b/sw/CycloneDDS/src/Loopback/loopback.c @@ -0,0 +1,138 @@ +#include "dds/dds.h" +#include "Type1.h" +#include +#include + +#define MAX_SAMPLES 1 + +int main (int argc, char ** argv){ + dds_qos_t* qos; + dds_entity_t domain; + dds_entity_t participant; + dds_entity_t topic; + dds_entity_t writer; + dds_entity_t reader; + dds_return_t rc; + dds_instance_handle_t inst1, inst2; + void *samples[MAX_SAMPLES]; + dds_sample_info_t infos[MAX_SAMPLES]; + Type1 msg; + Type1* msgp; + uint32_t status = 0; + (void)argc; + (void)argv; + + // Create Domain + domain = dds_create_domain(1, " many cyclonedds.pcap finest "); + if (domain < 0) { + DDS_FATAL("dds_create_domain: %s\n", dds_strretcode(-domain)); + } + + //Create QoS + qos = dds_create_qos(); + if (qos == NULL) { + DDS_FATAL("dds_create_qos failed\n"); + } + + //**RxO QoS** + dds_qset_durability(qos, DDS_DURABILITY_TRANSIENT_LOCAL); + dds_qset_durability_service(qos, 0, DDS_HISTORY_KEEP_LAST, 5, DDS_LENGTH_UNLIMITED, DDS_LENGTH_UNLIMITED, DDS_LENGTH_UNLIMITED); + //dds_qset_presentation(qos, DDS_PRESENTATION_INSTANCE, FALSE, FALSE); + //dds_qset_deadline(qos, DDS_INFINITY); + //dds_qset_latency_budget(qos, 0); + //dds_qset_ownership(qos, DDS_OWNERSHIP_SHARED); + //dds_qset_liveliness(qos, DDS_LIVELINESS_AUTOMATIC, DDS_INFINITY); + //dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_MSECS(100)); + //dds_qset_destination_order(qos, DDS_DESTINATIONORDER_BY_RECEPTION_TIMESTAMP); + + //dds_qset_data_representation(qos, 1, (dds_data_representation_id_t[]) { DDS_DATA_REPRESENTATION_XCDR1 }); + dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, 5); + dds_qset_resource_limits(qos, 20, 5, 20); + + //Create a Participant + participant = dds_create_participant(1, qos, NULL); + if (participant < 0) { + DDS_FATAL("dds_create_participant: %s\n", dds_strretcode(-participant)); + } + + //Create a Topic + topic = dds_create_topic(participant, &Type1_desc, "Topic1", qos, NULL); + if (topic < 0) { + DDS_FATAL("dds_create_topic: %s\n", dds_strretcode(-topic)); + } + + //Create a Writer + writer = dds_create_writer(participant, topic, qos, NULL); + if (writer < 0) { + DDS_FATAL("dds_create_writer: %s\n", dds_strretcode(-writer)); + } + + //Create a Reader + reader = dds_create_reader (participant, topic, qos, NULL); + if (reader < 0) { + DDS_FATAL("dds_create_reader: %s\n", dds_strretcode(-reader)); + } + + // Register Instance + msg.id = 2; + rc = dds_register_instance(writer, &inst2, &msg); + if (rc != DDS_RETCODE_OK) { + DDS_FATAL("dds_register_instance: %s\n", dds_strretcode(-rc)); + } + + // Write 5 samples with Instance 1 (msg.id=1) + printf ("=== [Publisher] Writing:\n"); + for (int i = 0; i < 5; i++) { + //Create a message to write + msg.id = 1; + msg.a = i; + + printf ("Message id:%d, a:%d\n", msg.id, msg.a); + fflush (stdout); + + rc = dds_write (writer, &msg); + if (rc != DDS_RETCODE_OK) { + DDS_FATAL("dds_write: %s\n", dds_strretcode(-rc)); + } + } + + // Initialize Buffer + samples[0] = Type1__alloc(); + + // Read 5 samples with Instance 2 (msg.id=2) + printf ("=== [Subscriber] Received :\n"); + for (int i = 0; i < 5;) { + + rc = dds_take(reader, samples, infos, MAX_SAMPLES, MAX_SAMPLES); + // Error + if (rc < 0) { + DDS_FATAL("dds_read: %s\n", dds_strretcode(-rc)); + } + // Sample Available (Only select relevant Instance) + else if ((rc > 0) && (infos[0].instance_handle == inst2) && infos[0].valid_data) { + msgp = (Type1*)samples[0]; + printf ("Message id:%d, a:%d\n", msgp->id, msgp->a); + fflush (stdout); + i++; + } + // No Data + else { + // Polling sleep + dds_sleepfor(DDS_MSECS(20)); + } + } + + // Free buffer + Type1_free(samples[0], DDS_FREE_ALL); + + // Delete QoS + dds_delete_qos(qos); + + // Deleting the participant will delete all its children recursively as well + rc = dds_delete(participant); + if (rc != DDS_RETCODE_OK) { + DDS_FATAL("dds_delete: %s\n", dds_strretcode(-rc)); + } + + return EXIT_SUCCESS; +} diff --git a/sw/CycloneDDS/src/helloworld/.gitignore b/sw/CycloneDDS/src/helloworld/.gitignore new file mode 100644 index 0000000..f65519e --- /dev/null +++ b/sw/CycloneDDS/src/helloworld/.gitignore @@ -0,0 +1 @@ +build/** diff --git a/sw/CycloneDDS/src/helloworld/CMakeLists.txt b/sw/CycloneDDS/src/helloworld/CMakeLists.txt new file mode 100644 index 0000000..461f209 --- /dev/null +++ b/sw/CycloneDDS/src/helloworld/CMakeLists.txt @@ -0,0 +1,33 @@ +# +# Copyright(c) 2021 ADLINK Technology Limited and others +# +# This program and the accompanying materials are made available under the +# terms of the Eclipse Public License v. 2.0 which is available at +# http://www.eclipse.org/legal/epl-2.0, or the Eclipse Distribution License +# v. 1.0 which is available at +# http://www.eclipse.org/org/documents/edl-v10.php. +# +# SPDX-License-Identifier: EPL-2.0 OR BSD-3-Clause +# +project(helloword LANGUAGES C) +cmake_minimum_required(VERSION 3.5) + +if(NOT TARGET CycloneDDS::ddsc) + # Find the CycloneDDS package. + find_package(CycloneDDS REQUIRED) +endif() + +# This is a convenience function, provided by the CycloneDDS package, +# that will supply a library target related the the given idl file. +# In short, it takes the idl file, generates the source files with +# the proper data types and compiles them into a library. +idlc_generate(TARGET HelloWorldData_lib FILES "HelloWorldData.idl") + +# Both executables have only one related source file. +add_executable(HelloworldPublisher publisher.c) +add_executable(HelloworldSubscriber subscriber.c) + +# Both executables need to be linked to the idl data type library and +# the ddsc API library. +target_link_libraries(HelloworldPublisher HelloWorldData_lib CycloneDDS::ddsc) +target_link_libraries(HelloworldSubscriber HelloWorldData_lib CycloneDDS::ddsc) diff --git a/sw/CycloneDDS/src/helloworld/HelloWorldData.idl b/sw/CycloneDDS/src/helloworld/HelloWorldData.idl new file mode 100644 index 0000000..0061d60 --- /dev/null +++ b/sw/CycloneDDS/src/helloworld/HelloWorldData.idl @@ -0,0 +1,9 @@ +module HelloWorldData +{ + struct Msg + { + long userID; + string message; + }; + #pragma keylist Msg userID +}; diff --git a/sw/CycloneDDS/src/helloworld/publisher.c b/sw/CycloneDDS/src/helloworld/publisher.c new file mode 100644 index 0000000..d1d469b --- /dev/null +++ b/sw/CycloneDDS/src/helloworld/publisher.c @@ -0,0 +1,74 @@ +#include "dds/dds.h" +#include "HelloWorldData.h" +#include +#include + +int main (int argc, char ** argv) +{ + dds_entity_t domain; + dds_entity_t participant; + dds_entity_t topic; + dds_entity_t writer; + dds_return_t rc; + HelloWorldData_Msg msg; + uint32_t status = 0; + (void)argc; + (void)argv; + + /* Create a Domain. */ + domain = dds_create_domain(1, " cyclonedds.pcap finest "); + if (domain < 0) + DDS_FATAL("dds_create_domain: %s\n", dds_strretcode(-domain)); + + /* Create a Participant. */ + participant = dds_create_participant (1, NULL, NULL); + if (participant < 0) + DDS_FATAL("dds_create_participant: %s\n", dds_strretcode(-participant)); + + /* Create a Topic. */ + topic = dds_create_topic ( + participant, &HelloWorldData_Msg_desc, "HelloWorldData_Msg", NULL, NULL); + if (topic < 0) + DDS_FATAL("dds_create_topic: %s\n", dds_strretcode(-topic)); + + /* Create a Writer. */ + writer = dds_create_writer (participant, topic, NULL, NULL); + if (writer < 0) + DDS_FATAL("dds_create_writer: %s\n", dds_strretcode(-writer)); + + printf("=== [Publisher] Waiting for a reader to be discovered ...\n"); + fflush (stdout); + + rc = dds_set_status_mask(writer, DDS_PUBLICATION_MATCHED_STATUS); + if (rc != DDS_RETCODE_OK) + DDS_FATAL("dds_set_status_mask: %s\n", dds_strretcode(-rc)); + + while(!(status & DDS_PUBLICATION_MATCHED_STATUS)) + { + rc = dds_get_status_changes (writer, &status); + if (rc != DDS_RETCODE_OK) + DDS_FATAL("dds_get_status_changes: %s\n", dds_strretcode(-rc)); + + /* Polling sleep. */ + dds_sleepfor (DDS_MSECS (20)); + } + + /* Create a message to write. */ + msg.userID = 1; + msg.message = "Hello World"; + + printf ("=== [Publisher] Writing : "); + printf ("Message (%"PRId32", %s)\n", msg.userID, msg.message); + fflush (stdout); + + rc = dds_write (writer, &msg); + if (rc != DDS_RETCODE_OK) + DDS_FATAL("dds_write: %s\n", dds_strretcode(-rc)); + + /* Deleting the participant will delete all its children recursively as well. */ + rc = dds_delete (participant); + if (rc != DDS_RETCODE_OK) + DDS_FATAL("dds_delete: %s\n", dds_strretcode(-rc)); + + return EXIT_SUCCESS; +} diff --git a/sw/CycloneDDS/src/helloworld/readme.rst b/sw/CycloneDDS/src/helloworld/readme.rst new file mode 100644 index 0000000..80e24bf --- /dev/null +++ b/sw/CycloneDDS/src/helloworld/readme.rst @@ -0,0 +1,46 @@ +.. + Copyright(c) 2006 to 2018 ADLINK Technology Limited and others + + This program and the accompanying materials are made available under the + terms of the Eclipse Public License v. 2.0 which is available at + http://www.eclipse.org/legal/epl-2.0, or the Eclipse Distribution License + v. 1.0 which is available at + http://www.eclipse.org/org/documents/edl-v10.php. + + SPDX-License-Identifier: EPL-2.0 OR BSD-3-Clause + +HelloWorld +========== + +Description +*********** + +The basic HelloWorld example is used to illustrate the necessary steps to setup DCPS entities. +Note it is also used in the Getting Started Guide to explain the usage of Eclipse Cyclone DDS. + +Design +****** + +It consists of 2 units: + +- HelloworldPublisher: implements the publisher's main +- HelloworldSubscriber: implements the subscriber's main + +Scenario +******** + +The publisher sends a single HelloWorld sample. The sample contains two fields: + +- a userID field (long type) +- a message field (string type) + +When it receives the sample, the subscriber displays the userID and the message field. + +Running the example +******************* + +It is recommended that you run the subscriber and publisher in separate terminals to avoid mixing the output. + +- Open 2 terminals. +- In the first terminal start the subscriber by running HelloWorldSubscriber +- In the second terminal start the publisher by running HelloWorldPublisher diff --git a/sw/CycloneDDS/src/helloworld/subscriber.c b/sw/CycloneDDS/src/helloworld/subscriber.c new file mode 100644 index 0000000..c15c270 --- /dev/null +++ b/sw/CycloneDDS/src/helloworld/subscriber.c @@ -0,0 +1,84 @@ +#include "dds/dds.h" +#include "HelloWorldData.h" +#include +#include +#include + +/* An array of one message (aka sample in dds terms) will be used. */ +#define MAX_SAMPLES 1 + +int main (int argc, char ** argv) +{ + dds_entity_t participant; + dds_entity_t topic; + dds_entity_t reader; + HelloWorldData_Msg *msg; + void *samples[MAX_SAMPLES]; + dds_sample_info_t infos[MAX_SAMPLES]; + dds_return_t rc; + dds_qos_t *qos; + (void)argc; + (void)argv; + + /* Create a Participant. */ + participant = dds_create_participant (1, NULL, NULL); + if (participant < 0) + DDS_FATAL("dds_create_participant: %s\n", dds_strretcode(-participant)); + + /* Create a Topic. */ + topic = dds_create_topic ( + participant, &HelloWorldData_Msg_desc, "HelloWorldData_Msg", NULL, NULL); + if (topic < 0) + DDS_FATAL("dds_create_topic: %s\n", dds_strretcode(-topic)); + + /* Create a reliable Reader. */ + qos = dds_create_qos (); + dds_qset_reliability (qos, DDS_RELIABILITY_RELIABLE, DDS_SECS (10)); + reader = dds_create_reader (participant, topic, qos, NULL); + if (reader < 0) + DDS_FATAL("dds_create_reader: %s\n", dds_strretcode(-reader)); + dds_delete_qos(qos); + + printf ("\n=== [Subscriber] Waiting for a sample ...\n"); + fflush (stdout); + + /* Initialize sample buffer, by pointing the void pointer within + * the buffer array to a valid sample memory location. */ + samples[0] = HelloWorldData_Msg__alloc (); + + /* Poll until data has been read. */ + while (true) + { + /* Do the actual read. + * The return value contains the number of read samples. */ + rc = dds_read (reader, samples, infos, MAX_SAMPLES, MAX_SAMPLES); + if (rc < 0) + DDS_FATAL("dds_read: %s\n", dds_strretcode(-rc)); + + /* Check if we read some data and it is valid. */ + if ((rc > 0) && (infos[0].valid_data)) + { + /* Print Message. */ + msg = (HelloWorldData_Msg*) samples[0]; + printf ("=== [Subscriber] Received : "); + printf ("Message (%"PRId32", %s)\n", msg->userID, msg->message); + fflush (stdout); + break; + } + else + { + /* Polling sleep. */ + dds_sleepfor (DDS_MSECS (20)); + } + } + + /* Free the data location. */ + HelloWorldData_Msg_free (samples[0], DDS_FREE_ALL); + + /* Deleting the participant will delete all its children recursively as well. */ + rc = dds_delete (participant); + if (rc != DDS_RETCODE_OK) + DDS_FATAL("dds_delete: %s\n", dds_strretcode(-rc)); + + return EXIT_SUCCESS; +} diff --git a/sw/ros/.gitignore b/sw/ros/.gitignore new file mode 100644 index 0000000..7dbd8c4 --- /dev/null +++ b/sw/ros/.gitignore @@ -0,0 +1,8 @@ +#Ignore Everything +* + +#WHITELIST +!.gitignore +!README.txt +!src +!src/** diff --git a/sw/ros/README.txt b/sw/ros/README.txt new file mode 100644 index 0000000..7ba5f29 --- /dev/null +++ b/sw/ros/README.txt @@ -0,0 +1,50 @@ +Tested with 'ros:galactic-ros-base' Docker Image + +Open ROS Docker Container inside /test directory + +Install missing dependencies (May need "apt update" before, sudo pass is "x11docker") +$ apt update +$ rosdep update --rosdistro=galactic +$ rosdep install -i --from-path src --rosdistro galactic -y + +Build ROS packages +$ colcon build + +Source Setup Files +$ . install/setup.bash + +CPP_SRVCLI +---------- + +$ export CYCLONEDDS_URI='manyfinest/home/john64/Desktop/rtps-fpga/download/rtps-test/ros/cyclonedds.log' +$ export CYCLONEDDS_URI='finest/home/john64/Desktop/rtps-fpga/download/rtps-test/ros/cyclonedds.log' + +$ ros2 run cpp_srvcli server + +$ ros2 run cpp_srvcli client 2 3 + +ACTION_TUTORIALS_CPP +-------------------- + +$ export CYCLONEDDS_URI='many' + +$ ros2 run action_tutorials_cpp fibonacci_action_server + +$ ros2 run action_tutorials_cpp fibonacci_action_client + +CPP_LOOPBACK +------------ + +$ export CYCLONEDDS_URI='manyfinest/home/john64/Desktop/rtps-fpga/download/rtps-test/ros/cyclonedds.log' +$ export CYCLONEDDS_URI='many' + +$ ros2 run cpp_loopback server + +$ ros2 run cpp_loopback client + + + + +MISC +==== +x11docker -it --network=host --sudouser --home='/home/john64' -- '--' 'ros:galactic-ros-base' 'bash' diff --git a/sw/ros/src/action_tutorials_cpp/CMakeLists.txt b/sw/ros/src/action_tutorials_cpp/CMakeLists.txt new file mode 100644 index 0000000..a1ede84 --- /dev/null +++ b/sw/ros/src/action_tutorials_cpp/CMakeLists.txt @@ -0,0 +1,98 @@ +cmake_minimum_required(VERSION 3.8) +project(action_tutorials_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(action_tutorials_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +add_library( + action_server + SHARED + src/fibonacci_action_server.cpp +) +add_library( + action_client + SHARED + src/fibonacci_action_client.cpp +) +target_include_directories( + action_server + PRIVATE + $ + $ +) +target_include_directories( + action_client + PRIVATE + $ + $ +) +target_compile_definitions( + action_server + PRIVATE + "ACTION_TUTORIALS_CPP_BUILDING_DLL" +) +target_compile_definitions( + action_client + PRIVATE + "ACTION_TUTORIALS_CPP_BUILDING_DLL" +) +ament_target_dependencies( + action_server + "action_tutorials_interfaces" + "rclcpp" + "rclcpp_action" + "rclcpp_components" +) +ament_target_dependencies( + action_client + "action_tutorials_interfaces" + "rclcpp" + "rclcpp_action" + "rclcpp_components" +) +rclcpp_components_register_node( + action_server + PLUGIN + "action_tutorials_cpp::FibonacciActionServer" + EXECUTABLE + fibonacci_action_server +) +rclcpp_components_register_node( + action_client + PLUGIN + "action_tutorials_cpp::FibonacciActionClient" + EXECUTABLE + fibonacci_action_client +) +install( + TARGETS + action_server + action_client + ARCHIVE DESTINATION + lib + LIBRARY DESTINATION + lib + RUNTIME DESTINATION + bin +) + +ament_package() diff --git a/sw/ros/src/action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h b/sw/ros/src/action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h new file mode 100644 index 0000000..94157f4 --- /dev/null +++ b/sw/ros/src/action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h @@ -0,0 +1,44 @@ +#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ +#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport)) + #define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport)) + #else + #define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport) + #define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport) + #endif + #ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL + #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT + #else + #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT + #endif + #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC + #define ACTION_TUTORIALS_CPP_LOCAL +#else + #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default"))) + #define ACTION_TUTORIALS_CPP_IMPORT + #if __GNUC__ >= 4 + #define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default"))) + #define ACTION_TUTORIALS_CPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ACTION_TUTORIALS_CPP_PUBLIC + #define ACTION_TUTORIALS_CPP_LOCAL + #endif + #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_ diff --git a/sw/ros/src/action_tutorials_cpp/package.xml b/sw/ros/src/action_tutorials_cpp/package.xml new file mode 100644 index 0000000..87e008b --- /dev/null +++ b/sw/ros/src/action_tutorials_cpp/package.xml @@ -0,0 +1,23 @@ + + + + action_tutorials_cpp + 0.0.0 + TODO: Package description + john64 + TODO: License declaration + + ament_cmake + + action_tutorials_interfaces + rclcpp + rclcpp_action + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sw/ros/src/action_tutorials_cpp/src/fibonacci_action_client.cpp b/sw/ros/src/action_tutorials_cpp/src/fibonacci_action_client.cpp new file mode 100644 index 0000000..aec0b91 --- /dev/null +++ b/sw/ros/src/action_tutorials_cpp/src/fibonacci_action_client.cpp @@ -0,0 +1,110 @@ +#include +#include +#include +#include +#include + +#include "action_tutorials_interfaces/action/fibonacci.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace action_tutorials_cpp +{ +class FibonacciActionClient : public rclcpp::Node +{ +public: + using Fibonacci = action_tutorials_interfaces::action::Fibonacci; + using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle; + + explicit FibonacciActionClient(const rclcpp::NodeOptions & options) + : Node("fibonacci_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client(this,"fibonacci"); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&FibonacciActionClient::send_goal, this)); + } + + void send_goal() + { + using namespace std::placeholders; + + this->timer_->cancel(); + + if (!this->client_ptr_->wait_for_action_server()) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = Fibonacci::Goal(); + goal_msg.order = 10; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&FibonacciActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&FibonacciActionClient::result_callback, this, _1); + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } + +private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::TimerBase::SharedPtr timer_; + + void goal_response_callback(std::shared_future future) + { + auto goal_handle = future.get(); + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + } + + void feedback_callback( + GoalHandleFibonacci::SharedPtr, + const std::shared_ptr feedback) + { + std::stringstream ss; + ss << "Next number in sequence received: "; + for (auto number : feedback->partial_sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + } + + void result_callback(const GoalHandleFibonacci::WrappedResult & result) + { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + std::stringstream ss; + ss << "Result received: "; + for (auto number : result.result->sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + rclcpp::shutdown(); + } +}; // class FibonacciActionClient + +} // namespace action_tutorials_cpp + +RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient) diff --git a/sw/ros/src/action_tutorials_cpp/src/fibonacci_action_server.cpp b/sw/ros/src/action_tutorials_cpp/src/fibonacci_action_server.cpp new file mode 100644 index 0000000..26eba4a --- /dev/null +++ b/sw/ros/src/action_tutorials_cpp/src/fibonacci_action_server.cpp @@ -0,0 +1,100 @@ +#include +#include +#include + +#include "action_tutorials_interfaces/action/fibonacci.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include "action_tutorials_cpp/visibility_control.h" + +namespace action_tutorials_cpp +{ +class FibonacciActionServer : public rclcpp::Node +{ +public: + using Fibonacci = action_tutorials_interfaces::action::Fibonacci; + using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle; + + ACTION_TUTORIALS_CPP_PUBLIC + explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("fibonacci_action_server", options) + { + using namespace std::placeholders; + + this->action_server_ = rclcpp_action::create_server( + this, + "fibonacci", + std::bind(&FibonacciActionServer::handle_goal, this, _1, _2), + std::bind(&FibonacciActionServer::handle_cancel, this, _1), + std::bind(&FibonacciActionServer::handle_accepted, this, _1)); + } + +private: + rclcpp_action::Server::SharedPtr action_server_; + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order); + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(const std::shared_ptr goal_handle) + { + using namespace std::placeholders; + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach(); + } + + void execute(const std::shared_ptr goal_handle) + { + RCLCPP_INFO(this->get_logger(), "Executing goal"); + rclcpp::Rate loop_rate(1); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto & sequence = feedback->partial_sequence; + sequence.push_back(0); + sequence.push_back(1); + auto result = std::make_shared(); + + for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + // Check if there is a cancel request + if (goal_handle->is_canceling()) { + result->sequence = sequence; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal canceled"); + return; + } + // Update sequence + sequence.push_back(sequence[i] + sequence[i - 1]); + // Publish feedback + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), "Publish feedback"); + + loop_rate.sleep(); + } + + // Check if goal is done + if (rclcpp::ok()) { + result->sequence = sequence; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } + } +}; // class FibonacciActionServer + +} // namespace action_tutorials_cpp + +RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer) diff --git a/sw/ros/src/cpp_loopback/CMakeLists.txt b/sw/ros/src/cpp_loopback/CMakeLists.txt new file mode 100644 index 0000000..e30f3d3 --- /dev/null +++ b/sw/ros/src/cpp_loopback/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(cpp_loopback) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tutorial_interfaces REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +add_executable(server src/loopback_server.cpp) +ament_target_dependencies(server rclcpp tutorial_interfaces) + +add_executable(client src/loopback_client.cpp) +ament_target_dependencies(client rclcpp tutorial_interfaces) + +install(TARGETS + server + client + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/sw/ros/src/cpp_loopback/package.xml b/sw/ros/src/cpp_loopback/package.xml new file mode 100644 index 0000000..d27daef --- /dev/null +++ b/sw/ros/src/cpp_loopback/package.xml @@ -0,0 +1,21 @@ + + + + cpp_loopback + 0.0.1 + A Loopback implementation using ROS Publisher/Subscriber used to compare the RTT between SW and FPGA implementation. + john64 + TODO: License declaration + + ament_cmake + + rclcpp + tutorial_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sw/ros/src/cpp_loopback/src/loopback_client.cpp b/sw/ros/src/cpp_loopback/src/loopback_client.cpp new file mode 100644 index 0000000..b920335 --- /dev/null +++ b/sw/ros/src/cpp_loopback/src/loopback_client.cpp @@ -0,0 +1,89 @@ +#include "rclcpp/rclcpp.hpp" +#include "rmw/types.h" +#include "tutorial_interfaces/msg/type1.hpp" +#include "rcl/time.h" + +rclcpp::Node::SharedPtr node; +rclcpp::Publisher::SharedPtr pub; +rclcpp::Subscription::SharedPtr sub; +rclcpp::Clock::SharedPtr steady_clock; +rclcpp::TimerBase::SharedPtr timer; + +void sub_callback(const tutorial_interfaces::msg::Type1 & msg){ + if (msg.id == 1){ + // Send Response + auto resp = tutorial_interfaces::msg::Type1(); + resp.id = 2; + resp.a = msg.a; + pub->publish(resp); + } + else { + RCLCPP_WARN(node->get_logger(), "Unexpected ID received: %d", msg.id); + } +} + +void timer_callback(){ + auto resp = tutorial_interfaces::msg::Type1(); + // Publish Input Utilization + resp.id = 3; + resp.a = 16384; + pub->publish(resp); + // Publish Ouput Utilization + resp.id = 4; + resp.a = 8192; + pub->publish(resp); +} + +int main(int argc, char **argv){ + rclcpp::init(argc, argv); + + //Create Node + rclcpp::NodeOptions node_options = rclcpp::NodeOptions(); + node_options.use_global_arguments(false); + node_options.enable_rosout(false); + node_options.use_intra_process_comms(false); + node_options.enable_topic_statistics(false); + node_options.start_parameter_services(false); + node_options.start_parameter_event_publisher(false); + node_options.use_clock_thread(false); + node_options.allow_undeclared_parameters(false); + node_options.automatically_declare_parameters_from_overrides(false); + + node = rclcpp::Node::make_shared("loopback_client",node_options); + + //Create Executor + rclcpp::executors::StaticSingleThreadedExecutor executor; + executor.add_node(node); + + //Create QoS + const rmw_qos_profile_t custom_qos_profile = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 10, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false + }; + const rclcpp::QoS custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos_profile),custom_qos_profile); + + //Create Publisher + pub = node->create_publisher("Loopback_2",custom_qos); + + //Create Subscriber + sub = node->create_subscription("Loopback_1",custom_qos, std::bind(&sub_callback,std::placeholders::_1)); + + //Create Clock + steady_clock = rclcpp::Clock::make_shared(RCL_STEADY_TIME); + + //Create Timer + timer = rclcpp::create_timer(node,steady_clock,std::chrono::nanoseconds(4096 * 4096 * 20),std::bind(&timer_callback)); + + //DONE + RCLCPP_INFO(node->get_logger(), "Spinning Node %s", node->get_name()); + executor.spin(); + RCLCPP_INFO(node->get_logger(), "Node %s exited. Shuting down", node->get_name()); + rclcpp::shutdown(); +} diff --git a/sw/ros/src/cpp_loopback/src/loopback_server.cpp b/sw/ros/src/cpp_loopback/src/loopback_server.cpp new file mode 100644 index 0000000..acaf41f --- /dev/null +++ b/sw/ros/src/cpp_loopback/src/loopback_server.cpp @@ -0,0 +1,210 @@ +#include +#include "rclcpp/rclcpp.hpp" +#include "rmw/types.h" +#include "tutorial_interfaces/msg/type1.hpp" +#include "rcl/time.h" + +#define MAX_MAP_SIZE 10 +#define INPUT_FIFO_SIZE 16384 +#define OUTPUT_FIFO_SIZE 16384 +#define ENABLE_AVERAGE true +#define DEBUG true + +rclcpp::Node::SharedPtr node; +rclcpp::Publisher::SharedPtr pub; +rclcpp::Subscription::SharedPtr sub; +rclcpp::Clock::SharedPtr steady_clock; +rclcpp::TimerBase::SharedPtr timer1; +rclcpp::TimerBase::SharedPtr timer2; +std::map start_time_map; +std::shared_mutex mutex1; +std::shared_mutex mutex2; +rclcpp::CallbackGroup::SharedPtr cb_group1; +rclcpp::CallbackGroup::SharedPtr cb_group2; +int avg_count[3] = {0, 0, 0}; +long long avg_acc[3] = {0, 0, 0}; + +void sub_callback(const tutorial_interfaces::msg::Type1 & msg){ + switch(msg.id){ + case 2:{ + mutex1.lock(); + std::map::iterator iter = start_time_map.find(msg.a); + if(iter != start_time_map.end()) { + rclcpp::Duration rtt = steady_clock->now() - iter->second; + start_time_map.erase(iter); + mutex1.unlock(); + if(ENABLE_AVERAGE){ + mutex2.lock(); + avg_acc[0] += rtt.nanoseconds(); + avg_count[0]++; + mutex2.unlock(); + } else { + RCLCPP_INFO(node->get_logger(), "Message: %d RTT: %ld ns", msg.a, rtt.nanoseconds()); + } + + } else { + if (false){ + RCLCPP_INFO(node->get_logger(), "Current contents of start_time_map:"); + for(auto it = start_time_map.cbegin(); it != start_time_map.cend(); ++it){ + RCLCPP_INFO(node->get_logger(), "A: %d", it->first); + } + } + mutex1.unlock(); + RCLCPP_WARN(node->get_logger(), "A: %d not found in start_time_map", msg.a); + } + break; + } + case 3:{ + //INPUT + if(ENABLE_AVERAGE){ + mutex2.lock(); + avg_acc[1] += msg.a; + avg_count[1]++; + mutex2.unlock(); + } else { + float util = ((INPUT_FIFO_SIZE - msg.a) * 100.0) / INPUT_FIFO_SIZE; + RCLCPP_INFO(node->get_logger(), "Input Utilization: %f %%", util); + } + break; + } + case 4:{ + //OUTPUT + if(ENABLE_AVERAGE){ + mutex2.lock(); + avg_acc[2] += msg.a; + avg_count[2]++; + mutex2.unlock(); + } else { + float util = ((OUTPUT_FIFO_SIZE - msg.a) * 100.0) / OUTPUT_FIFO_SIZE; + RCLCPP_INFO(node->get_logger(), "Output Utilization: %f %%", util); + } + break; + } + default:{ + RCLCPP_WARN(node->get_logger(), "Unexpected ID received: %d", msg.id); + break; + } + } +} + +void main_loop() { + //Cancel Timer + timer1->cancel(); + + //Wait for Server + RCLCPP_INFO(node->get_logger(), "Waiting for client"); + while (node->count_subscribers("Loopback_2") < 1 || node->count_publishers("Loopback_1") < 1){ + rclcpp::sleep_for(std::chrono::seconds(1)); + } + RCLCPP_INFO(node->get_logger(), "Client endpoints available."); + + uint32_t index = 0; + + //Main Loop + while (rclcpp::ok()) { + mutex1.lock(); + long size = start_time_map.size(); + mutex1.unlock(); + if (size >= MAX_MAP_SIZE) { + continue; + } + auto resp = tutorial_interfaces::msg::Type1(); + // Publish Input Utilization + resp.id = 1; + resp.a = index; + mutex1.lock(); + pub->publish(resp); + rclcpp::Time t = steady_clock->now(); + start_time_map[index] = t; + mutex1.unlock(); + index++; + //rclcpp::sleep_for(std::chrono::nanoseconds(500)); + } +} + +void average(){ + long avg[3]; + int cnt = avg_count[0]; + + mutex2.lock(); + for (int i = 0; i < 3; i++){ + if (avg_count[i] != 0){ + avg[i] = avg_acc[i] / avg_count[i]; + } else { + avg[i] = 0; + } + //Reset + avg_acc[i] = 0; + avg_count[i] = 0; + } + mutex2.unlock(); + RCLCPP_INFO(node->get_logger(), "Average message RTT: %ld ns, Throughput: %d", avg[0], cnt); + RCLCPP_INFO(node->get_logger(), "Input Utilization: %f %%", ((INPUT_FIFO_SIZE - avg[1]) * 100.0) / INPUT_FIFO_SIZE); + RCLCPP_INFO(node->get_logger(), "Output Utilization: %f %%", ((OUTPUT_FIFO_SIZE - avg[2]) * 100.0) / OUTPUT_FIFO_SIZE); + if(DEBUG) { + mutex1.lock(); + RCLCPP_INFO(node->get_logger(), "Map Size: %ld", start_time_map.size()); + mutex1.unlock(); + } +} + +int main(int argc, char **argv){ + rclcpp::init(argc, argv); + + //Create Node + rclcpp::NodeOptions node_options = rclcpp::NodeOptions(); + node_options.use_global_arguments(false); + node_options.enable_rosout(false); + node_options.use_intra_process_comms(false); + node_options.enable_topic_statistics(false); + node_options.start_parameter_services(false); + node_options.start_parameter_event_publisher(false); + node_options.use_clock_thread(false); + node_options.allow_undeclared_parameters(false); + node_options.automatically_declare_parameters_from_overrides(false); + + node = rclcpp::Node::make_shared("loopback_server",node_options); + + //Create Executor + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + + //Create QoS + const rmw_qos_profile_t custom_qos_profile = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 10, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false + }; + const rclcpp::QoS custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos_profile),custom_qos_profile); + + //Create Publisher + pub = node->create_publisher("Loopback_1",custom_qos); + + //Create Subscriber + sub = node->create_subscription("Loopback_2",custom_qos, std::bind(&sub_callback,std::placeholders::_1)); + + //Create Clock + steady_clock = rclcpp::Clock::make_shared(RCL_STEADY_TIME); + + //Create Callbackgroups + cb_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cb_group2 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + //Create Timer + timer1 = rclcpp::create_timer(node,steady_clock,std::chrono::seconds(1),std::bind(&main_loop),cb_group1); + if(ENABLE_AVERAGE) { + timer2 = rclcpp::create_timer(node,steady_clock,std::chrono::seconds(1),std::bind(&average),cb_group2); + } + + //DONE + RCLCPP_INFO(node->get_logger(), "Spinning Node %s", node->get_name()); + executor.spin(); + RCLCPP_INFO(node->get_logger(), "Node %s exited. Shuting down", node->get_name()); + rclcpp::shutdown(); +} diff --git a/sw/ros/src/cpp_srvcli/CMakeLists.txt b/sw/ros/src/cpp_srvcli/CMakeLists.txt new file mode 100644 index 0000000..be7f4b5 --- /dev/null +++ b/sw/ros/src/cpp_srvcli/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.8) +project(cpp_srvcli) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(example_interfaces REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +add_executable(server src/add_two_ints_server.cpp) +ament_target_dependencies(server rclcpp example_interfaces) + +add_executable(client src/add_two_ints_client.cpp) +ament_target_dependencies(client rclcpp example_interfaces) + + +install( + TARGETS + server + client + DESTINATION + lib/${PROJECT_NAME} +) + +ament_package() diff --git a/sw/ros/src/cpp_srvcli/package.xml b/sw/ros/src/cpp_srvcli/package.xml new file mode 100644 index 0000000..34001d7 --- /dev/null +++ b/sw/ros/src/cpp_srvcli/package.xml @@ -0,0 +1,21 @@ + + + + cpp_srvcli + 0.0.0 + TODO: Package description + john64 + TODO: License declaration + + ament_cmake + + rclcpp + example_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sw/ros/src/cpp_srvcli/src/add_two_ints_client.cpp b/sw/ros/src/cpp_srvcli/src/add_two_ints_client.cpp new file mode 100644 index 0000000..4fd57f1 --- /dev/null +++ b/sw/ros/src/cpp_srvcli/src/add_two_ints_client.cpp @@ -0,0 +1,55 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + if (argc != 3) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "usage: add_two_ints_client X Y"); + return 1; + } + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_client"); + rclcpp::Client::SharedPtr client = + node->create_client("add_two_ints"); + + auto request = std::make_shared(); + request->a = atoll(argv[1]); + request->b = atoll(argv[2]); + + while (!client->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, waiting again..."); + } + + /* + NOTE + + There is a race condition happening with CycloneDDS, where if a Sample/Heartbeat arrives before CycloneDDS had time + to store the remote endpoint, the sample/SN is discarded. + In order to prevent that we add another sleep to make sure that CycloneDDS has discovered all remote endpoints + before the service request is sent out. + */ + rclcpp::sleep_for(std::chrono::seconds(1)); + + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sum: %ld", result.get()->sum); + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_two_ints"); + } + + rclcpp::shutdown(); + return 0; +} diff --git a/sw/ros/src/cpp_srvcli/src/add_two_ints_server.cpp b/sw/ros/src/cpp_srvcli/src/add_two_ints_server.cpp new file mode 100644 index 0000000..03e14d4 --- /dev/null +++ b/sw/ros/src/cpp_srvcli/src/add_two_ints_server.cpp @@ -0,0 +1,24 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +#include + +void add(const std::shared_ptr request, std::shared_ptr response){ +response->sum = request->a + request->b; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\na: %ld" " b: %ld", request->a, request->b); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "sending back response: [%ld]", (long int)response->sum); +} + +int main(int argc, char **argv){ + rclcpp::init(argc, argv); + + std::shared_ptr node = rclcpp::Node::make_shared("add_two_ints_server"); + + rclcpp::Service::SharedPtr service = + node->create_service("add_two_ints", &add); + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints."); + + rclcpp::spin(node); + rclcpp::shutdown(); +} diff --git a/sw/ros/src/tutorial_interfaces/CMakeLists.txt b/sw/ros/src/tutorial_interfaces/CMakeLists.txt new file mode 100644 index 0000000..c704162 --- /dev/null +++ b/sw/ros/src/tutorial_interfaces/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(tutorial_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Type1.msg" +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/sw/ros/src/tutorial_interfaces/msg/Type1.msg b/sw/ros/src/tutorial_interfaces/msg/Type1.msg new file mode 100644 index 0000000..f30b9d3 --- /dev/null +++ b/sw/ros/src/tutorial_interfaces/msg/Type1.msg @@ -0,0 +1,2 @@ +uint32 id +uint32 a diff --git a/sw/ros/src/tutorial_interfaces/package.xml b/sw/ros/src/tutorial_interfaces/package.xml new file mode 100644 index 0000000..4886ede --- /dev/null +++ b/sw/ros/src/tutorial_interfaces/package.xml @@ -0,0 +1,20 @@ + + + + tutorial_interfaces + 0.0.0 + TODO: Package description + john64 + TODO: License declaration + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sw/standalone/test_fpga.c b/sw/standalone/test_fpga.c new file mode 100644 index 0000000..9ae10f3 --- /dev/null +++ b/sw/standalone/test_fpga.c @@ -0,0 +1,123 @@ +#include //printf +#include //open +#include //open +#include //open +#include //close +#include //perror +#include //exit +#include //mmap +#include //signal +#include //assert +#include //socket, inet_aton +#include //socket, inet_aton +#include //socket +#include //htons, ntohs, htonl, ntohl, inet_aton + +#include "hps_0.h" + +#define LW_FPGA_SLAVES_BASE 0xFF200000 +#define LW_FPGA_SVAVES_END 0xFF3FFFFF +#define LW_FPGA_SLAVES_SPAN 0x00200000 + +void bail(); +void sig_handler(int signum); + +//NOTE: use void* Pointer to avoid pointer arithmetic during offset addition +void* lw_bridge = NULL; +int mem_fd = -1; + +int main(int argc, char** argv){ + + //Register Signal Handling + signal(SIGHUP, sig_handler); + signal(SIGINT, sig_handler); + signal(SIGQUIT, sig_handler); + signal(SIGABRT, sig_handler); + + mem_fd = open("/dev/mem", O_RDWR | O_SYNC); + if(mem_fd < 0) { + perror("Could not open /dev/mem"); + bail(EXIT_FAILURE); + } + + lw_bridge = (uint32_t*)mmap(NULL, LW_FPGA_SLAVES_SPAN, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, LW_FPGA_SLAVES_BASE); + if(lw_bridge == MAP_FAILED) { + perror("Could not mmap /dev/mem"); + bail(EXIT_FAILURE); + } + + // Polling Address + uint32_t* ADDR_0 = (uint32_t*)(lw_bridge + TEST_FPGA_0_BASE); + // Read Address + uint32_t* ADDR_1 = ADDR_0 + 1; //Pointer Arithmetic + // Write Address + uint32_t* ADDR_2 = ADDR_0 + 2; //Pointer Arithmetic + + struct sockaddr_in src, dest; + + src.sin_family = AF_INET; + src.sin_port = htons(5000); + inet_aton("192.168.0.10", &(src.sin_addr)); + + dest.sin_family = AF_INET; + dest.sin_port = htons(4000); + inet_aton("192.168.0.15", &(dest.sin_addr)); + + uint32_t tmp; + struct in_addr tmp_addr; + + assert(*(ADDR_0) == 0); + // Write SRC Address + *ADDR_2 = src.sin_addr.s_addr; + // Write DEST Address + *ADDR_2 = dest.sin_addr.s_addr; + // Write UDP Ports + *ADDR_2 = ((src.sin_port << 16) & 0xFFFF0000) | (dest.sin_port & 0xFFFF); + // Write Packet Length + *ADDR_2 = htonl(2); + // Write BYTE 1 + *ADDR_2 = 0xAAAABBBB; + // Write BYTE 2 + *ADDR_2 = 0xCCCCDDDD; + + // Read SRC Address + tmp_addr.s_addr = *ADDR_1; + printf("SRC Address: %s\n", inet_ntoa(tmp_addr)); + assert(*ADDR_0 == 1); + // Read DEST Address + tmp_addr.s_addr = *ADDR_1; + printf("DEST Address: %s\n", inet_ntoa(tmp_addr)); + // Read UDP Ports + tmp = *ADDR_1; + printf("UDP SRC Port: %d, UDP DEST Port: %d\n", ntohs((tmp >> 16) & 0xFFFF), ntohs(tmp & 0xFFFF)); + // Read Packet Length + printf("Packet Length: %d\n", ntohl(*ADDR_1)); + // Read BYTE1 + printf("Byte 1: %X\n", *ADDR_1); + // Read BYTE2 + printf("Byte 2: %X\n", *ADDR_1); + // Read BYTE3 + printf("Byte 3: %X\n", *ADDR_1); + + assert(*(ADDR_0) == 0); + printf("Done.\n"); + exit(EXIT_SUCCESS); +} + +void bail(int code){ + // Close File Descriptor + if (mem_fd != -1){ + close(mem_fd); + } + + // Unmap /dev/mem + if (lw_bridge != NULL){ + munmap(lw_bridge, LW_FPGA_SLAVES_SPAN); + } + exit(code); +} + +void sig_handler(int signum){ + printf("Received Signal %d.\n Exiting...\n", signum); + bail(EXIT_SUCCESS); +} diff --git a/sw/standalone/udp_client.cpp b/sw/standalone/udp_client.cpp new file mode 100644 index 0000000..17bdcdc --- /dev/null +++ b/sw/standalone/udp_client.cpp @@ -0,0 +1,121 @@ +#include //socket, inet_aton +#include //socket, inet_aton +#include //socket +#include //htons, ntohs, htonl, ntohl, inet_aton +#include //signal +#include //fprintf +#include //perror +#include //strtol +#include //close + +/*PROTOTYPES*/ +void bail(int code); +void sig_handler(int signum); +void usage(char* name); + +/*DEFINES*/ +//Magic Number used to distinguish our UDP Packets (First 8 Bytes of Payload) +#define MAGIC 0xDEADBEEFCAFEFEED +//Receive Buffer Size +#define BUFFER_SIZE 1000 + +/*GLOBAL VARIABLES*/ +long port = 0; +int sock_fd = -1; +struct sockaddr_in src_addr; + +int main(int argc, char **argv){ + + /*Parse Arguments*/ + if (argc < 3) { + fprintf(stderr, "Insufficient arguments\n"); + usage(argv[0]); + } + + //Source Address + in_addr_t src_ip_addr = inet_addr(argv[1]); + if (src_ip_addr == INADDR_NONE) { + fprintf(stderr, "Invalid SRC IP Address\n"); + usage(argv[0]); + } + + //Port Number + errno = 0; + port = strtol(argv[2], NULL, 10); + if (errno != 0) { + fprintf(stderr, "Invalid Port Number\n"); + usage(argv[0]); + } + + /*MISC MAIN THREAD*/ + //Register Signal Handling + signal(SIGHUP, sig_handler); + signal(SIGINT, sig_handler); + signal(SIGQUIT, sig_handler); + signal(SIGABRT, sig_handler); + + /*SOCKET OPERATIONS*/ + src_addr = {AF_INET, htons(port), {src_ip_addr}}; + + //Open Socket + sock_fd = socket(AF_INET, SOCK_DGRAM, 0); + //Bind Socket + if (bind(sock_fd, (const struct sockaddr*) &src_addr, sizeof(src_addr)) < 0) { + perror("Bind failed"); + bail(EXIT_FAILURE); + } + //Set Receive Timeout + const struct timeval tv = {.tv_usec = 1000}; //1 ms + setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + + char buffer[BUFFER_SIZE]; + uint64_t* p = (uint64_t*) buffer; + socklen_t addrlen; + struct sockaddr_in src; + int n; + + printf("Entering Main loop\n"); + + while(true){ + //Receive Packet + n = recvfrom(sock_fd, buffer, BUFFER_SIZE, MSG_TRUNC, (struct sockaddr*) &src, &addrlen); + if (n < 0){ + // Error + if (errno != EAGAIN && errno != EWOULDBLOCK) { + perror("rcvfrom() error"); + } + continue; + } + //Packet not relevant + if (src.sin_family != src_addr.sin_family && src.sin_port != src_addr.sin_port && n >= 8 && p[0] != MAGIC){ + continue; + } + //Sent Packet Back + if (sendto(sock_fd, buffer, n, 0, (const struct sockaddr*) &src, addrlen) < 0) { + perror("sendto failed"); + } + } + +} + +void bail(int code){ + //Close Sockets + if (sock_fd != -1) { + close(sock_fd); + } + //DONE + exit(code); +} + +void sig_handler(int signum){ + fprintf(stderr, "Received Signal %d.\n Exiting...\n", signum); + bail(EXIT_SUCCESS); +} + +void usage(char* name){ + printf("USAGE: %s SRC_ADDRESS PORT\n", name); + printf("SRC_ADDRESS The local source IPv4 Address from which to send and listen to (In x.x.x.x format)\n"); + printf("PORT The port number of the Addresses\n"); + + bail(EXIT_SUCCESS); +} diff --git a/sw/standalone/udp_forwarder.MT.c b/sw/standalone/udp_forwarder.MT.c new file mode 100644 index 0000000..69b395c --- /dev/null +++ b/sw/standalone/udp_forwarder.MT.c @@ -0,0 +1,402 @@ +#include //printf +#include //open +#include //open +#include //open +#include //close, getopt +#include //perror +#include //exit +#include //mmap +#include //signal +#include //assert +#include //socket, inet_aton +#include //socket, inet_aton +#include //socket +#include //htons, ntohs, htonl, ntohl, inet_aton +#include //nanosleep +#include //bool +#include //Multithreading +#include "hps_0.h" + +/* +DEVELOPER NOTE + +This is an attempt to make udp_forwarder multithreaded, but for some reason I cannot make it faster than the single thread variant. + +Initially I thought the slow down was due to reading and writing to/from the FPGA happening at the same time, since +there is only one AXI master (meaning that only one operation can happen at any time) and mixing of read and write +operations somehow interferes with each other. +But adding mutexes makes it actually slower! + +After much experimenting this are the results I got: +* Single Thread Variant + 16k packets/s +* Multi Thread Variant + 7.1k packets/s +* Multi Thread Variant with no sleep + 13.6k packets/s +* Multi Thread Variant with mutexes + 6.9k packets/s +* Multi Thread Variant with mutexes and no sleep + 12.1k packets/s +*/ + +/*PROTOTYPES*/ +void usage(char* name); +void bail(int code); +void sig_handler(int signum); +void* send_thread(void* vargp); + +/*DEFINES*/ +#define LW_FPGA_SLAVES_BASE 0xFF200000 +#define LW_FPGA_SLAVES_END 0xFF3FFFFF +#define LW_FPGA_SLAVES_SPAN 0x00200000 +#define BUF_LENGTH 65536 +#define NUM_SOCKETS 5 +#define PORT_CONFIG_PB 7400 +#define PORT_CONFIG_DG 250 +#define PORT_CONFIG_PG 2 +#define PORT_CONFIG_D0 0 +#define PORT_CONFIG_D1 10 +#define PORT_CONFIG_D2 1 +#define PORT_CONFIG_D3 11 + +/*GLOBAL VARIABLES*/ +//NOTE: use void* Pointer to avoid pointer arithmetic during offset addition +volatile sig_atomic_t done = false; +void* lw_bridge = NULL; +int mem_fd = -1; +int sock_fd[NUM_SOCKETS]; +uint8_t endian = -1; //0 Big Endian, 1 Little Endian +in_addr_t src_addr; +long src_did; +bool quiet = false; +struct sockaddr_in bind_addr[NUM_SOCKETS]; +uint32_t* ADDR_0; +uint32_t* ADDR_1; +uint32_t* ADDR_2; +pthread_mutex_t mutex; + +/*CONSTANTS*/ +const struct timespec ms = {0, 10^6}; // 1ms +const struct timespec us = {0, 10^3}; // 1us +const struct timespec sec = {1, 0}; // 1sec + +int main(int argc, char** argv) { + + int opt; + + while ((opt = getopt(argc, argv, "q")) != -1) { + switch (opt) { + case 'q': + quiet = true; + break; + default: + /* invalid option */ + fprintf(stderr, "Invalid Option\n"); + usage(argv[0]); + break; + } + } + + + if (argc-optind < 2) { + fprintf(stderr, "Insufficient arguments\n"); + usage(argv[0]); + } + + src_addr = inet_addr(argv[optind]); + if (src_addr == INADDR_NONE) { + fprintf(stderr, "Invalid IP Address\n"); + usage(argv[0]); + } + + errno = 0; + src_did = strtol(argv[optind+1], NULL, 10); + if (errno != 0) { + fprintf(stderr, "Invalid Domain ID\n"); + usage(argv[0]); + } + + // Initialize Sockets to aid bail function + for (int i = 0; i < NUM_SOCKETS; i++) { + sock_fd[i] = -1; + } + + //Register Signal Handling + signal(SIGHUP, sig_handler); + signal(SIGINT, sig_handler); + signal(SIGQUIT, sig_handler); + signal(SIGABRT, sig_handler); + + // Check Endianness + { + int i = 1; + endian = (* (char*) &i == 1) ? 1 : 0; + printf("ENDIANNESS: %d\n", endian); + } + + if (endian != 1) { + fprintf(stderr, "Program is tested on Little Endian Systems\n"); + bail(EXIT_FAILURE); + } + + mem_fd = open("/dev/mem", O_RDWR | O_SYNC); + if (mem_fd < 0) { + perror("Could not open /dev/mem"); + bail(EXIT_FAILURE); + } + + lw_bridge = (uint32_t*)mmap(NULL, LW_FPGA_SLAVES_SPAN, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, LW_FPGA_SLAVES_BASE); + if (lw_bridge == MAP_FAILED) { + perror("Could not mmap /dev/mem"); + bail(EXIT_FAILURE); + } + + // Polling Address + ADDR_0 = (uint32_t*)(lw_bridge + TEST_FPGA_0_BASE); + // Read Address + ADDR_1 = ADDR_0 + 1; //Pointer Arithmetic + // Write Address + ADDR_2 = ADDR_0 + 2; //Pointer Arithmetic + + for (int i = 0; i < NUM_SOCKETS; i++) { + sock_fd[i] = socket(AF_INET, SOCK_DGRAM, 0); + if (sock_fd[i] < 0) { + fprintf(stderr, "Socket %d", i); + perror("Could not open socket"); + bail(EXIT_FAILURE); + } + } + + bind_addr[0] = (struct sockaddr_in){AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D0 + PORT_CONFIG_DG * src_did), {inet_addr("239.255.0.1")}}; + bind_addr[1] = (struct sockaddr_in){AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D0 + PORT_CONFIG_DG * src_did), {src_addr}}; + bind_addr[2] = (struct sockaddr_in){AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D1 + PORT_CONFIG_DG * src_did), {src_addr}}; + bind_addr[3] = (struct sockaddr_in){AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D2 + PORT_CONFIG_DG * src_did), {src_addr}}; + bind_addr[4] = (struct sockaddr_in){AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D3 + PORT_CONFIG_DG * src_did), {src_addr}}; + printf("bind_addr[0]: %s:%d\n",inet_ntoa(bind_addr[0].sin_addr),ntohs(bind_addr[0].sin_port)); + printf("bind_addr[1]: %s:%d\n",inet_ntoa(bind_addr[1].sin_addr),ntohs(bind_addr[1].sin_port)); + printf("bind_addr[2]: %s:%d\n",inet_ntoa(bind_addr[2].sin_addr),ntohs(bind_addr[2].sin_port)); + printf("bind_addr[3]: %s:%d\n",inet_ntoa(bind_addr[3].sin_addr),ntohs(bind_addr[3].sin_port)); + printf("bind_addr[4]: %s:%d\n",inet_ntoa(bind_addr[4].sin_addr),ntohs(bind_addr[4].sin_port)); + + for (int i = 0; i < NUM_SOCKETS; i++) { + if (bind(sock_fd[i], (const struct sockaddr*) &bind_addr[i], sizeof(bind_addr[i])) < 0) { + fprintf(stderr, "bind_addr[%d]", i); + perror("Bind failed"); + bail(EXIT_FAILURE); + } + } + + if (pthread_mutex_init(&mutex,NULL) != 0){ + perror("pthread_mutex_init failed"); + bail(EXIT_FAILURE); + } + + pthread_t thread_id; + if (pthread_create(&thread_id, NULL, send_thread, NULL) != 0){ + perror("pthread_create failed"); + bail(EXIT_FAILURE); + } + + struct sockaddr_in src, dest; + char buffer[BUF_LENGTH]; + int n, addrlen, j; + uint32_t* p; + + printf("Entering Receive Loop\n"); + + while (!done) { + /*UDP INPUT*/ + for (j = 0; j < NUM_SOCKETS; j++) { + addrlen = sizeof(src); + n = recvfrom(sock_fd[j], buffer, BUF_LENGTH, MSG_DONTWAIT | MSG_TRUNC, (struct sockaddr*) &src, &addrlen); + if (n < 0) { + // Error + if (errno != EAGAIN && errno != EWOULDBLOCK) { + perror("rcvfrom() error"); + break; + } + } + // Exit Condition + else if (n > 0) { + break; + } + } + + if (n > 0) { + //Buffer not 4-Byte aligned + if ((n % 4) != 0) { + // Add zero byte padding + for(int offset = 4 - (n % 4); offset > 0; offset--){ + buffer[n+offset-1] = 0; + } + // Convert n to Word Count + n = (n / 4) + 1; + } + else { + // Convert n to Word Count + n = n / 4; + } + //SANITY CHECK + if (addrlen != sizeof(src)){ + fprintf(stderr, "rcvfrom() returned unexpected addrlen.\n"); + continue; + } + /* + while (pthread_mutex_lock(&mutex) != 0){ + perror("pthread_mutex_lock failed"); + } + */ + // Write SRC Address + *ADDR_2 = src.sin_addr.s_addr; + // Write DEST Address + *ADDR_2 = bind_addr[j].sin_addr.s_addr; + // Write UDP Ports + *ADDR_2 = ((bind_addr[j].sin_port << 16) & 0xFFFF0000) | (src.sin_port & 0xFFFF); + // Write Packet Length + *ADDR_2 = htonl(n); + // Write Packet + p = (uint32_t*) buffer; + for (int i = 0; i < n; i++) { + *ADDR_2 = p[i]; + } + /* + while (pthread_mutex_unlock(&mutex) != 0){ + perror("pthread_mutex_lock failed"); + } + */ + if (!quiet){ + printf("Packet received (%d Words)\n", n); + printf(" Src: %s:%d\n",inet_ntoa(bind_addr[j].sin_addr),ntohs(bind_addr[j].sin_port)); + } + } + /* + else { + nanosleep(&us, NULL); //Wait 1 us + } + */ + } + printf("Receive Loop exited.\n"); + + printf("Waiting for threads...\n"); + pthread_join(thread_id, NULL); + + bail(EXIT_SUCCESS); +} + +void* send_thread(void* vargp){ + + struct sockaddr_in src, dest; + char buffer[BUF_LENGTH]; + int n, addrlen, j; + uint32_t* p; + uint32_t tmp; + + printf("Entering Send Loop\n"); + + while (!done) { + /*UDP OUTPUT*/ + // FPGA has output + /* + while (pthread_mutex_lock(&mutex) != 0){ + perror("pthread_mutex_lock failed"); + } + */ + if (*ADDR_0 != 0) { + src.sin_family = AF_INET; + dest.sin_family = AF_INET; + // Read SRC Address + src.sin_addr.s_addr = *ADDR_1; + // Read DEST Address + dest.sin_addr.s_addr = *ADDR_1; + // Read UDP Ports + tmp = *ADDR_1; + dest.sin_port = ((tmp >> 16) & 0xFFFF); + src.sin_port = (tmp & 0xFFFF); + // Read Packet Length + n = ntohl(*ADDR_1); + // Read Packet + p = (uint32_t*) buffer; + for(int i = 0; i < n; i++) { + p[i] = *ADDR_1; + } + /* + while (pthread_mutex_unlock(&mutex) != 0){ + perror("pthread_mutex_lock failed"); + } + */ + // Convert n to Byte Count + n = n * 4; + + // Select correct socket + j = NUM_SOCKETS; + for (int i = 0; i < NUM_SOCKETS; i++) { + if (bind_addr[i].sin_addr.s_addr == src.sin_addr.s_addr && bind_addr[i].sin_port == src.sin_port) { + j = i; + break; + } + } + // No bind_addr Match + if (j == NUM_SOCKETS) { + fprintf(stderr, "Provided SRC does not have respective bind_addr\n"); + fprintf(stderr, "SRC: %s %d\n", inet_ntoa(src.sin_addr), ntohs(src.sin_port)); + continue; + } + + n = sendto(sock_fd[j], buffer, n, 0, (const struct sockaddr*) &dest, sizeof(dest)); + if (n < 0) { + perror("sendto failed"); + continue; + } + if (!quiet) { + printf("Packet sent (%d Words)\n", n/4); + printf(" Src: %s:%d\n",inet_ntoa(bind_addr[j].sin_addr),ntohs(bind_addr[j].sin_port)); + printf(" Dest: %s:%d\n",inet_ntoa(dest.sin_addr),ntohs(dest.sin_port)); + } + } + /* + else { + while (pthread_mutex_unlock(&mutex) != 0){ + perror("pthread_mutex_lock failed"); + } + } + */ + /* + nanosleep(&us, NULL); //Wait 1 us + */ + } + printf("Send Loop exited.\n"); +} + +void bail(int code){ + // Close File Descriptors + if (mem_fd != -1){ + close(mem_fd); + } + for (int i = 0; i < NUM_SOCKETS; i++) { + if (sock_fd[i] != -1) { + close(sock_fd[i]); + } + } + + // Unmap /dev/mem + if (lw_bridge != NULL){ + munmap(lw_bridge, LW_FPGA_SLAVES_SPAN); + } + exit(code); +} + +void sig_handler(int signum){ + fprintf(stderr, "Received Signal %d.\n Exiting...\n", signum); + done = true; +} + +void usage(char* name){ + printf("USAGE: %s [-q] ADDRESS DOMAIN_ID\n", name); + printf("-q Quiet Flag. When specified, no information is printed during the main loops\n"); + printf("ADDRESS The SRC IPv4 Address of the FPGA implementation (In x.x.x.x format)\n"); + printf(" NOTE: The system has to have a interface with the same address\n"); + printf("DOMAIN_ID The Domain ID of the FPGA implementation\n"); + + bail(EXIT_SUCCESS); +} diff --git a/sw/standalone/udp_forwarder.c b/sw/standalone/udp_forwarder.c new file mode 100644 index 0000000..9bda536 --- /dev/null +++ b/sw/standalone/udp_forwarder.c @@ -0,0 +1,315 @@ +#include //printf +#include //open +#include //open +#include //open +#include //close, getopt +#include //perror +#include //exit +#include //mmap +#include //signal +#include //assert +#include //socket, inet_aton +#include //socket, inet_aton +#include //socket +#include //htons, ntohs, htonl, ntohl, inet_aton +#include // nanosleep +#include // bool +#include "hps_0.h" + +/*PROTOTYPES*/ +void usage(char* name); +void bail(int code); +void sig_handler(int signum); + +/*DEFINES*/ +#define LW_FPGA_SLAVES_BASE 0xFF200000 +#define LW_FPGA_SLAVES_END 0xFF3FFFFF +#define LW_FPGA_SLAVES_SPAN 0x00200000 +#define BUF_LENGTH 65536 +#define NUM_SOCKETS 5 +#define PORT_CONFIG_PB 7400 +#define PORT_CONFIG_DG 250 +#define PORT_CONFIG_PG 2 +#define PORT_CONFIG_D0 0 +#define PORT_CONFIG_D1 10 +#define PORT_CONFIG_D2 1 +#define PORT_CONFIG_D3 11 + +/*GLOBAL VARIABLES*/ +//NOTE: use void* Pointer to avoid pointer arithmetic during offset addition +void* lw_bridge = NULL; +int mem_fd = -1; +int sock_fd[NUM_SOCKETS]; +uint8_t endian = -1; //0 Big Endian, 1 Little Endian +in_addr_t src_addr; +long src_did; +bool quiet = false; + +/*CONSTANTS*/ +const struct timespec ms = {0, 10^6}; // 1ms +const struct timespec us = {0, 10^3}; // 1us +const struct timespec sec = {1, 0}; // 1sec + +int main(int argc, char** argv) { + + int opt; + + while ((opt = getopt(argc, argv, "q")) != -1) { + switch (opt) { + case 'q': + quiet = true; + break; + default: + /* invalid option */ + fprintf(stderr, "Invalid Option\n"); + usage(argv[0]); + break; + } + } + + + if (argc-optind < 2) { + fprintf(stderr, "Insufficient arguments\n"); + usage(argv[0]); + } + + src_addr = inet_addr(argv[optind]); + if (src_addr == INADDR_NONE) { + fprintf(stderr, "Invalid IP Address\n"); + usage(argv[0]); + } + + errno = 0; + src_did = strtol(argv[optind+1], NULL, 10); + if (errno != 0) { + fprintf(stderr, "Invalid Domain ID\n"); + usage(argv[0]); + } + + // Initialize Sockets to aid bail function + for (int i = 0; i < NUM_SOCKETS; i++) { + sock_fd[i] = -1; + } + + //Register Signal Handling + signal(SIGHUP, sig_handler); + signal(SIGINT, sig_handler); + signal(SIGQUIT, sig_handler); + signal(SIGABRT, sig_handler); + + // Check Endianness + { + int i = 1; + endian = (* (char*) &i == 1) ? 1 : 0; + printf("ENDIANNESS: %d\n", endian); + } + + if (endian != 1) { + fprintf(stderr, "Program is tested on Little Endian Systems\n"); + bail(EXIT_FAILURE); + } + + mem_fd = open("/dev/mem", O_RDWR | O_SYNC); + if (mem_fd < 0) { + perror("Could not open /dev/mem"); + bail(EXIT_FAILURE); + } + + lw_bridge = (uint32_t*)mmap(NULL, LW_FPGA_SLAVES_SPAN, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, LW_FPGA_SLAVES_BASE); + if (lw_bridge == MAP_FAILED) { + perror("Could not mmap /dev/mem"); + bail(EXIT_FAILURE); + } + + // Polling Address + uint32_t* ADDR_0 = (uint32_t*)(lw_bridge + TEST_FPGA_0_BASE); + // Read Address + uint32_t* ADDR_1 = ADDR_0 + 1; //Pointer Arithmetic + // Write Address + uint32_t* ADDR_2 = ADDR_0 + 2; //Pointer Arithmetic + + for (int i = 0; i < NUM_SOCKETS; i++) { + sock_fd[i] = socket(AF_INET, SOCK_DGRAM, 0); + if (sock_fd[i] < 0) { + fprintf(stderr, "Socket %d", i); + perror("Could not open socket"); + bail(EXIT_FAILURE); + } + } + + const struct sockaddr_in bind_addr[NUM_SOCKETS] = { + {AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D0 + PORT_CONFIG_DG * src_did), {inet_addr("239.255.0.1")}}, + {AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D0 + PORT_CONFIG_DG * src_did), {src_addr}}, + {AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D1 + PORT_CONFIG_DG * src_did), {src_addr}}, + {AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D2 + PORT_CONFIG_DG * src_did), {src_addr}}, + {AF_INET, htons(PORT_CONFIG_PB + PORT_CONFIG_D3 + PORT_CONFIG_DG * src_did), {src_addr}} + }; + printf("bind_addr[0]: %s:%d\n",inet_ntoa(bind_addr[0].sin_addr),ntohs(bind_addr[0].sin_port)); + printf("bind_addr[1]: %s:%d\n",inet_ntoa(bind_addr[1].sin_addr),ntohs(bind_addr[1].sin_port)); + printf("bind_addr[2]: %s:%d\n",inet_ntoa(bind_addr[2].sin_addr),ntohs(bind_addr[2].sin_port)); + printf("bind_addr[3]: %s:%d\n",inet_ntoa(bind_addr[3].sin_addr),ntohs(bind_addr[3].sin_port)); + printf("bind_addr[4]: %s:%d\n",inet_ntoa(bind_addr[4].sin_addr),ntohs(bind_addr[4].sin_port)); + + for (int i = 0; i < NUM_SOCKETS; i++) { + if (bind(sock_fd[i], (const struct sockaddr*) &bind_addr[i], sizeof(bind_addr[i])) < 0) { + fprintf(stderr, "bind_addr[%d]", i); + perror("Bind failed"); + bail(EXIT_FAILURE); + } + } + + struct sockaddr_in src, dest; + + char buffer[BUF_LENGTH]; + int n, addrlen, j; + uint32_t* p; + uint32_t tmp; + + printf("Entering Loop\n"); + + while (1) { + /*UDP OUTPUT*/ + // FPGA has output + if (*ADDR_0 != 0) { + src.sin_family = AF_INET; + dest.sin_family = AF_INET; + // Read SRC Address + src.sin_addr.s_addr = *ADDR_1; + // Read DEST Address + dest.sin_addr.s_addr = *ADDR_1; + // Read UDP Ports + tmp = *ADDR_1; + dest.sin_port = ((tmp >> 16) & 0xFFFF); + src.sin_port = (tmp & 0xFFFF); + // Read Packet Length + n = ntohl(*ADDR_1); + // Read Packet + p = (uint32_t*) buffer; + for(int i = 0; i < n; i++) { + p[i] = *ADDR_1; + } + // Convert n to Byte Count + n = n * 4; + + // Select correct socket + j = NUM_SOCKETS; + for (int i = 0; i < NUM_SOCKETS; i++) { + if (bind_addr[i].sin_addr.s_addr == src.sin_addr.s_addr && bind_addr[i].sin_port == src.sin_port) { + j = i; + break; + } + } + // No bind_addr Match + if (j == NUM_SOCKETS) { + fprintf(stderr, "Provided SRC does not have respective bind_addr\n"); + fprintf(stderr, "SRC: %s %d\n", inet_ntoa(src.sin_addr), ntohs(src.sin_port)); + bail(EXIT_FAILURE); + } + + n = sendto(sock_fd[j], buffer, n, 0, (const struct sockaddr*) &dest, sizeof(dest)); + if (n < 0) { + perror("sendto failed"); + bail(EXIT_FAILURE); + } + if (!quiet) { + printf("Packet sent (%d Words)\n", n/4); + printf(" Src: %s:%d\n",inet_ntoa(bind_addr[j].sin_addr),ntohs(bind_addr[j].sin_port)); + printf(" Dest: %s:%d\n",inet_ntoa(dest.sin_addr),ntohs(dest.sin_port)); + } + } + + + /*UDP INPUT*/ + for (j = 0; j < NUM_SOCKETS; j++) { + addrlen = sizeof(src); + n = recvfrom(sock_fd[j], buffer, BUF_LENGTH, MSG_DONTWAIT | MSG_TRUNC, (struct sockaddr*) &src, &addrlen); + if (n < 0) { + // Error + if (errno != EAGAIN && errno != EWOULDBLOCK) { + perror("rcvfrom() error"); + bail(EXIT_FAILURE); + } + } + // Exit Condition + else if (n > 0) { + break; + } + } + + if (n > 0) { + //Buffer not 4-Byte aligned + if ((n % 4) != 0) { + // Add zero byte padding + for(int offset = 4 - (n % 4); offset > 0; offset--){ + buffer[n+offset-1] = 0; + } + // Convert n to Word Count + n = (n / 4) + 1; + } + else { + // Convert n to Word Count + n = n / 4; + } + //SANITY CHECK + if (addrlen != sizeof(src)){ + fprintf(stderr, "rcvfrom() returned unexpected addrlen.\n"); + bail(EXIT_FAILURE); + } + // Write SRC Address + *ADDR_2 = src.sin_addr.s_addr; + // Write DEST Address + *ADDR_2 = bind_addr[j].sin_addr.s_addr; + // Write UDP Ports + *ADDR_2 = ((bind_addr[j].sin_port << 16) & 0xFFFF0000) | (src.sin_port & 0xFFFF); + // Write Packet Length + *ADDR_2 = htonl(n); + // Write Packet + p = (uint32_t*) buffer; + for (int i = 0; i < n; i++) { + *ADDR_2 = p[i]; + } + if (!quiet){ + printf("Packet received (%d Words)\n", n); + printf(" Src: %s:%d\n",inet_ntoa(bind_addr[j].sin_addr),ntohs(bind_addr[j].sin_port)); + } + } + /* + else { + nanosleep(&ms, NULL); //Wait 1 ms + } + */ + } +} + +void bail(int code){ + // Close File Descriptors + if (mem_fd != -1){ + close(mem_fd); + } + for (int i = 0; i < NUM_SOCKETS; i++) { + if (sock_fd[i] != -1) { + close(sock_fd[i]); + } + } + + // Unmap /dev/mem + if (lw_bridge != NULL){ + munmap(lw_bridge, LW_FPGA_SLAVES_SPAN); + } + exit(code); +} + +void sig_handler(int signum){ + fprintf(stderr, "Received Signal %d.\n Exiting...\n", signum); + bail(EXIT_SUCCESS); +} + +void usage(char* name){ + printf("USAGE: %s [-q] ADDRESS DOMAIN_ID\n", name); + printf("-q Quiet Flag. When specified, no information is printed during the main loops\n"); + printf("ADDRESS The SRC IPv4 Address of the FPGA implementation (In x.x.x.x format)\n"); + printf(" NOTE: The system has to have a interface with the same address\n"); + printf("DOMAIN_ID The Domain ID of the FPGA implementation\n"); + + bail(EXIT_SUCCESS); +} diff --git a/sw/standalone/udp_server.cpp b/sw/standalone/udp_server.cpp new file mode 100644 index 0000000..2ca5174 --- /dev/null +++ b/sw/standalone/udp_server.cpp @@ -0,0 +1,218 @@ +#include //socket, inet_aton +#include //socket, inet_aton +#include //socket +#include //htons, ntohs, htonl, ntohl, inet_aton +#include //signal +#include //std::thread +#include //std::unordered_set +#include //std::shared_mutex +#include //close + +/*PROTOTYPES*/ +void send_thread(); +void receive_thread(); +void bail(int code); +void sig_handler(int signum); +void usage(char* name); + +/*DEFINES*/ +//Magic Number used to distinguish our UDP Packets (First 8 Bytes of Payload) +#define MAGIC 0xDEADBEEFCAFEFEED +//Size Limit of set (This implicitly is the limit of in transit messages expected to return) [0 means no limit] +#define MSG_LIMIT 100 +//Payload size of UDP messages in Bytes [Has to be >= 16 Bytes] +#define PAYLOAD_SIZE 100 +//Print extra debug messages +#define DEBUG true + +/*GLOBAL VARIABLES*/ +long port = 0; +int sock_fd = -1; +volatile sig_atomic_t done = false; +std::unordered_set set; +std::shared_mutex mutex1; +std::shared_mutex mutex2; +struct sockaddr_in src_addr, dest_addr; +long pkt_cnt = 0; + +const struct timespec ms = {0, 10^6}; // 1ms +const struct timespec us = {0, 10^3}; // 1us +const struct timespec sec = {1, 0}; // 1sec + +int main(int argc, char **argv){ + + /*Parse Arguments*/ + if (argc < 4) { + fprintf(stderr, "Insufficient arguments\n"); + usage(argv[0]); + } + + //Source Address + in_addr_t src_ip_addr = inet_addr(argv[1]); + if (src_ip_addr == INADDR_NONE) { + fprintf(stderr, "Invalid SRC IP Address\n"); + usage(argv[0]); + } + //Destination Address + in_addr_t dest_ip_addr = inet_addr(argv[2]); + if (dest_ip_addr == INADDR_NONE) { + fprintf(stderr, "Invalid DEST IP Address\n"); + usage(argv[0]); + } + //Port Number + errno = 0; + port = strtol(argv[3], NULL, 10); + if (errno != 0) { + fprintf(stderr, "Invalid Port Number\n"); + usage(argv[0]); + } + + /*MISC MAIN THREAD*/ + //Register Signal Handling + signal(SIGHUP, sig_handler); + signal(SIGINT, sig_handler); + signal(SIGQUIT, sig_handler); + signal(SIGABRT, sig_handler); + + /*SOCKET OPERATIONS*/ + src_addr = {AF_INET, htons(port), {src_ip_addr}}; + dest_addr = {AF_INET, htons(port), {dest_ip_addr}}; + + //Open Socket + sock_fd = socket(AF_INET, SOCK_DGRAM, 0); + //Bind Socket + if (bind(sock_fd, (const struct sockaddr*) &src_addr, sizeof(src_addr)) < 0) { + perror("Bind failed"); + bail(EXIT_FAILURE); + } + //Set Receive Timeout + const struct timeval tv = {.tv_usec = 1000}; //1 ms + setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + + /*MAIN STUFF*/ + + //Start Threads + std::thread t1(send_thread); + std::thread t2(receive_thread); + + printf("Entering Main loop\n"); + + //MAIN LOOP + while(!done){ + //Sleep + nanosleep(&sec, NULL); //Wait 1 sec + //Read and reset Packet Counter + mutex2.lock(); + long cnt = pkt_cnt; + pkt_cnt = 0; + mutex2.unlock(); + //Print Results + printf("Throughput: %ld\n", cnt); + //Debug Messages + if(DEBUG){ + mutex1.lock(); + cnt = set.size(); + mutex1.unlock(); + printf("Current set size: %ld\n", cnt); + } + } + + printf("Main loop exited. Waiting for threads...\n"); + + //Wait for Threads + t1.join(); + t2.join(); + + bail(EXIT_SUCCESS); +} + +void send_thread(){ + uint64_t id = 0; + char buffer[PAYLOAD_SIZE]; + uint64_t* p = (uint64_t*) buffer; + long size; + + //Magic Number + p[0] = MAGIC; + + while(!done){ + mutex1.lock(); + set.insert(id); + size = set.size(); + mutex1.unlock(); + p[1] = id; + if (sendto(sock_fd, buffer, PAYLOAD_SIZE, 0, (const struct sockaddr*) &dest_addr, sizeof(dest_addr)) < 0) { + perror("sendto failed"); + mutex1.lock(); + set.erase(id); + mutex1.unlock(); + continue; + } + id++; + //Reached Message Limit + while (!done && MSG_LIMIT != 0 && size >= MSG_LIMIT){ + //Wait + nanosleep(&us, NULL); //Wait 1 us + mutex1.lock(); + size = set.size(); + mutex1.unlock(); + } + } +} + +void receive_thread(){ + char buffer[PAYLOAD_SIZE]; + uint64_t* p = (uint64_t*) buffer; + socklen_t addrlen; + struct sockaddr_in src; + + while(!done){ + //Receive + if (recvfrom(sock_fd, buffer, PAYLOAD_SIZE, MSG_TRUNC, (struct sockaddr*) &src, &addrlen) < 0){ + // Error + if (errno != EAGAIN && errno != EWOULDBLOCK) { + perror("rcvfrom() error"); + } + continue; + } + //Packet not relevant + if (addrlen != sizeof(dest_addr) && src.sin_family != dest_addr.sin_family && src.sin_addr.s_addr != dest_addr.sin_addr.s_addr && src.sin_port != dest_addr.sin_port && p[0] != MAGIC){ + continue; + } + mutex1.lock(); + auto it = set.find(p[1]); + if (it != set.end()){ + set.erase(it); + mutex1.unlock(); + mutex2.lock(); + pkt_cnt++; + mutex2.unlock(); + } else { + mutex1.unlock(); + fprintf(stderr, "ID not in set.\n"); + } + } +} + +void bail(int code){ + //Close Sockets + if (sock_fd != -1) { + close(sock_fd); + } + //DONE + exit(code); +} + +void sig_handler(int signum){ + fprintf(stderr, "Received Signal %d.\n Exiting...\n", signum); + done = true; +} + +void usage(char* name){ + printf("USAGE: %s SRC_ADDRESS DEST_ADDRESS PORT\n", name); + printf("SRC_ADDRESS The local source IPv4 Address from which to send and listen to (In x.x.x.x format)\n"); + printf("DEST_ADDRESS The remote destination IPv4 Address to which to send to (In x.x.x.x format)\n"); + printf("PORT The port number used for both Addresses\n"); + + bail(EXIT_SUCCESS); +} diff --git a/syn/DE10_NANO_SoC_GHRD/output_files/BACKUP/README.txt b/syn/DE10_NANO_SoC_GHRD/output_files/BACKUP/README.txt new file mode 100644 index 0000000..ea3b1bb --- /dev/null +++ b/syn/DE10_NANO_SoC_GHRD/output_files/BACKUP/README.txt @@ -0,0 +1,10 @@ +NAME RELATED_ENTITY RELATED_SW +test_fpga /syn/test_fpga.vhd /sw/standalone/test_fpga.c +dds_loopback /src/Tests/Level_2/L2_Testbench_Lib4.vhd /sw/CycloneDDS/src/Loopback +ros_service /src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib2.vhd /sw/ros/src/cpp_srvcli +ros_action /src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib4.vhd /sw/ros/src/action_tutorials_cpp +ros_rtt /src/ros2/Tests/Level_2/L2_Testbench_ROS_Lib6.vhd /sw/ros/src/cpp_loopback +loopback /syn/loopback.vhd /sw/standalone/udp_server.cpp + /sw/standalone/udp_client.cpp + +NOTE: /sw/standalone/udp_forwarder.c is to be used in the HPS to forward UDP Packets to/from the FPGA implementation.