greek64/rtps-fpga
2
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add sw Folder with related source code for practical testing

Browse Source
This commit is contained in:
John Ring 2023-07-30 16:41:56 +02:00
parent df67cf626d
commit d73fe79b60
38 changed files with 2657 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
sw/CycloneDDS/Dockerfile/Dockerfile Normal file
View File

@ -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"]

3
sw/CycloneDDS/Dockerfile/build.sh Normal file
View File

@ -0,0 +1,3 @@
#!/bin/sh
docker build --progress=plain --tag cyclonedds:0.8.0 ./

6
sw/CycloneDDS/Dockerfile/entrypoint.sh Normal file
View File

@ -0,0 +1,6 @@
#!/bin/bash
export PATH=/opt/CycloneDDS/bin:$PATH
export LD_LIBRARY_PATH=/opt/CycloneDDS/lib:$LD_LIBRARY_PATH
exec $1

20
sw/CycloneDDS/README.txt Normal file
View File

@ -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

1
sw/CycloneDDS/src/Loopback/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
build/**

31
sw/CycloneDDS/src/Loopback/CMakeLists.txt Normal file
View File

@ -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)

5
sw/CycloneDDS/src/Loopback/Type1.idl Normal file
View File

@ -0,0 +1,5 @@
@extensibility(FINAL)
struct Type1 {
@key long id;
long a;
};

138
sw/CycloneDDS/src/Loopback/loopback.c Normal file
View File

@ -0,0 +1,138 @@
#include "dds/dds.h"
#include "Type1.h"
#include <stdio.h>
#include <stdlib.h>
#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, "<CycloneDDS> <Domain> <Compatibility> <ManySocketsMode> many </ManySocketsMode> </Compatibility> <Tracing> <PacketCaptureFile> cyclonedds.pcap </PacketCaptureFile> <Verbosity> finest </Verbosity> </Tracing> </Domain> </CycloneDDS>");
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;
}

1
sw/CycloneDDS/src/helloworld/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
build/**

33
sw/CycloneDDS/src/helloworld/CMakeLists.txt Normal file
View File

@ -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)

9
sw/CycloneDDS/src/helloworld/HelloWorldData.idl Normal file
View File

@ -0,0 +1,9 @@
module HelloWorldData
{
struct Msg
{
long userID;
string message;
};
#pragma keylist Msg userID
};

74
sw/CycloneDDS/src/helloworld/publisher.c Normal file
View File

@ -0,0 +1,74 @@
#include "dds/dds.h"
#include "HelloWorldData.h"
#include <stdio.h>
#include <stdlib.h>
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> <Domain> <Tracing> <PacketCaptureFile> cyclonedds.pcap </PacketCaptureFile> <Verbosity> finest </Verbosity> </Tracing> </Domain> </CycloneDDS>");
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;
}

46
sw/CycloneDDS/src/helloworld/readme.rst Normal file
View File

@ -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

84
sw/CycloneDDS/src/helloworld/subscriber.c Normal file
View File

@ -0,0 +1,84 @@
#include "dds/dds.h"
#include "HelloWorldData.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
/* 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;
}

8
sw/ros/.gitignore vendored Normal file
View File

@ -0,0 +1,8 @@
#Ignore Everything
*
#WHITELIST
!.gitignore
!README.txt
!src
!src/**

50
sw/ros/README.txt Normal file
View File

@ -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='<Compatibility><ManySocketsMode>many</ManySocketsMode></Compatibility><Tracing><Verbosity>finest</Verbosity><Out>/home/john64/Desktop/rtps-fpga/download/rtps-test/ros/cyclonedds.log</Out></Tracing>'
$ export CYCLONEDDS_URI='<Tracing><Verbosity>finest</Verbosity><Out>/home/john64/Desktop/rtps-fpga/download/rtps-test/ros/cyclonedds.log</Out></Tracing>'
$ ros2 run cpp_srvcli server
$ ros2 run cpp_srvcli client 2 3
ACTION_TUTORIALS_CPP
--------------------
$ export CYCLONEDDS_URI='<Compatibility><ManySocketsMode>many</ManySocketsMode></Compatibility>'
$ ros2 run action_tutorials_cpp fibonacci_action_server
$ ros2 run action_tutorials_cpp fibonacci_action_client
CPP_LOOPBACK
------------
$ export CYCLONEDDS_URI='<Compatibility><ManySocketsMode>many</ManySocketsMode></Compatibility><Tracing><Verbosity>finest</Verbosity><Out>/home/john64/Desktop/rtps-fpga/download/rtps-test/ros/cyclonedds.log</Out></Tracing>'
$ export CYCLONEDDS_URI='<Compatibility><ManySocketsMode>many</ManySocketsMode></Compatibility>'
$ ros2 run cpp_loopback server
$ ros2 run cpp_loopback client
MISC
====
x11docker -it --network=host --sudouser --home='/home/john64' -- '--' 'ros:galactic-ros-base' 'bash'

98
sw/ros/src/action_tutorials_cpp/CMakeLists.txt Normal file
View File

@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_include_directories(
action_client
PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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()

44
sw/ros/src/action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h Normal file
View File

@ -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_

23
sw/ros/src/action_tutorials_cpp/package.xml Normal file
View File

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>action_tutorials_cpp</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="greek64.mail@gmail.com">john64</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>action_tutorials_interfaces</depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_components</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

110
sw/ros/src/action_tutorials_cpp/src/fibonacci_action_client.cpp Normal file
View File

@ -0,0 +1,110 @@
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#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<Fibonacci>;
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
{
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(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<Fibonacci>::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<Fibonacci>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> 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<const Fibonacci::Feedback> 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)

100
sw/ros/src/action_tutorials_cpp/src/fibonacci_action_server.cpp Normal file
View File

@ -0,0 +1,100 @@
#include <functional>
#include <memory>
#include <thread>
#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<Fibonacci>;
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<Fibonacci>(
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<Fibonacci>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> 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<GoalHandleFibonacci> 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<GoalHandleFibonacci> 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<GoalHandleFibonacci> 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<Fibonacci::Feedback>();
auto & sequence = feedback->partial_sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
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)

35
sw/ros/src/cpp_loopback/CMakeLists.txt Normal file
View File

@ -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()

21
sw/ros/src/cpp_loopback/package.xml Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cpp_loopback</name>
<version>0.0.1</version>
<description>A Loopback implementation using ROS Publisher/Subscriber used to compare the RTT between SW and FPGA implementation.</description>
<maintainer email="greek64.mail@gmail.com">john64</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>tutorial_interfaces</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

89
sw/ros/src/cpp_loopback/src/loopback_client.cpp Normal file
View File

@ -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<tutorial_interfaces::msg::Type1>::SharedPtr pub;
rclcpp::Subscription<tutorial_interfaces::msg::Type1>::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<tutorial_interfaces::msg::Type1>("Loopback_2",custom_qos);
//Create Subscriber
sub = node->create_subscription<tutorial_interfaces::msg::Type1>("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();
}

210
sw/ros/src/cpp_loopback/src/loopback_server.cpp Normal file
View File

@ -0,0 +1,210 @@
#include <map>
#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<tutorial_interfaces::msg::Type1>::SharedPtr pub;
rclcpp::Subscription<tutorial_interfaces::msg::Type1>::SharedPtr sub;
rclcpp::Clock::SharedPtr steady_clock;
rclcpp::TimerBase::SharedPtr timer1;
rclcpp::TimerBase::SharedPtr timer2;
std::map<int, rclcpp::Time> 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<int, rclcpp::Time>::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<tutorial_interfaces::msg::Type1>("Loopback_1",custom_qos);
//Create Subscriber
sub = node->create_subscription<tutorial_interfaces::msg::Type1>("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();
}

39
sw/ros/src/cpp_srvcli/CMakeLists.txt Normal file
View File

@ -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()

21
sw/ros/src/cpp_srvcli/package.xml Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cpp_srvcli</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="greek64.mail@gmail.com">john64</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>example_interfaces</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

55
sw/ros/src/cpp_srvcli/src/add_two_ints_client.cpp Normal file
View File

@ -0,0 +1,55 @@
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
#include <chrono>
#include <cstdlib>
#include <memory>
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<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_client");
rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client =
node->create_client<example_interfaces::srv::AddTwoInts>("add_two_ints");
auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>();
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;
}

24
sw/ros/src/cpp_srvcli/src/add_two_ints_server.cpp Normal file
View File

@ -0,0 +1,24 @@
#include "rclcpp/rclcpp.hpp"
#include "example_interfaces/srv/add_two_ints.hpp"
#include <memory>
void add(const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request, std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> 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<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_server");
rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr service =
node->create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", &add);
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ready to add two ints.");
rclcpp::spin(node);
rclcpp::shutdown();
}

27
sw/ros/src/tutorial_interfaces/CMakeLists.txt Normal file
View File

@ -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()

2
sw/ros/src/tutorial_interfaces/msg/Type1.msg Normal file
View File

@ -0,0 +1,2 @@
uint32 id
uint32 a

20
sw/ros/src/tutorial_interfaces/package.xml Normal file
View File

@ -0,0 +1,20 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>tutorial_interfaces</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="john64@todo.todo">john64</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

123
sw/standalone/test_fpga.c Normal file
View File

@ -0,0 +1,123 @@
#include <stdio.h> //printf
#include <sys/types.h> //open
#include <sys/stat.h> //open
#include <fcntl.h> //open
#include <unistd.h> //close
#include <errno.h> //perror
#include <stdlib.h> //exit
#include <sys/mman.h> //mmap
#include <signal.h> //signal
#include <assert.h> //assert
#include <sys/socket.h> //socket, inet_aton
#include <netinet/in.h> //socket, inet_aton
#include <netinet/udp.h> //socket
#include <arpa/inet.h> //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);
}

121
sw/standalone/udp_client.cpp Normal file
View File

@ -0,0 +1,121 @@
#include <sys/socket.h> //socket, inet_aton
#include <netinet/in.h> //socket, inet_aton
#include <netinet/udp.h> //socket
#include <arpa/inet.h> //htons, ntohs, htonl, ntohl, inet_aton
#include <signal.h> //signal
#include <stdio.h> //fprintf
#include <errno.h> //perror
#include <stdlib.h> //strtol
#include <unistd.h> //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);
}

402
sw/standalone/udp_forwarder.MT.c Normal file
View File

@ -0,0 +1,402 @@
#include <stdio.h> //printf
#include <sys/types.h> //open
#include <sys/stat.h> //open
#include <fcntl.h> //open
#include <unistd.h> //close, getopt
#include <errno.h> //perror
#include <stdlib.h> //exit
#include <sys/mman.h> //mmap
#include <signal.h> //signal
#include <assert.h> //assert
#include <sys/socket.h> //socket, inet_aton
#include <netinet/in.h> //socket, inet_aton
#include <netinet/udp.h> //socket
#include <arpa/inet.h> //htons, ntohs, htonl, ntohl, inet_aton
#include <time.h> //nanosleep
#include <stdbool.h> //bool
#include <pthread.h> //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);
}

315
sw/standalone/udp_forwarder.c Normal file
View File

@ -0,0 +1,315 @@
#include <stdio.h> //printf
#include <sys/types.h> //open
#include <sys/stat.h> //open
#include <fcntl.h> //open
#include <unistd.h> //close, getopt
#include <errno.h> //perror
#include <stdlib.h> //exit
#include <sys/mman.h> //mmap
#include <signal.h> //signal
#include <assert.h> //assert
#include <sys/socket.h> //socket, inet_aton
#include <netinet/in.h> //socket, inet_aton
#include <netinet/udp.h> //socket
#include <arpa/inet.h> //htons, ntohs, htonl, ntohl, inet_aton
#include <time.h> // nanosleep
#include <stdbool.h> // 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);
}

218
sw/standalone/udp_server.cpp Normal file
View File

@ -0,0 +1,218 @@
#include <sys/socket.h> //socket, inet_aton
#include <netinet/in.h> //socket, inet_aton
#include <netinet/udp.h> //socket
#include <arpa/inet.h> //htons, ntohs, htonl, ntohl, inet_aton
#include <signal.h> //signal
#include <thread> //std::thread
#include <unordered_set> //std::unordered_set
#include <shared_mutex> //std::shared_mutex
#include <unistd.h> //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<uint64_t> 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);
}

10
syn/DE10_NANO_SoC_GHRD/output_files/BACKUP/README.txt Normal file
View File

@ -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.