diff --git a/CMakeLists.txt b/CMakeLists.txt index 91ea359dd86ff9549fa6c6f4cafb9bf4b3d5c221..2a437ef6fdcbc4175dac46bfca7e19f815103fd6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,11 @@ find_package(catkin REQUIRED roscpp) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) -find_package(nng CONFIG REQUIRED) + +# make sure NNG is updated in the submodule +execute_process(COMMAND git submodule update --init -- lib/nng +WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(lib/nng) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed @@ -100,7 +104,7 @@ find_package(nng CONFIG REQUIRED) ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES nng_test + LIBRARIES nng_test # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib ) @@ -146,9 +150,11 @@ set_target_properties(${PROJECT_NAME}_receiver PROPERTIES OUTPUT_NAME receiver P ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_sender ${catkin_LIBRARIES} + nng ) target_link_libraries(${PROJECT_NAME}_receiver ${catkin_LIBRARIES} + nng ) ############# diff --git a/package.xml b/package.xml index afd98f6fee454a7866e81d9093824e328ccc57ae..8ed139dd011ced9fb3bdb40477f3026209db8f44 100644 --- a/package.xml +++ b/package.xml @@ -5,8 +5,6 @@ <description>a sender and receiver node using nng</description> <!-- One maintainer tag required, multiple allowed, one person per tag --> - <!-- Example: --> - <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> <maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer> @@ -25,7 +23,7 @@ <!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Authors do not have to be maintainers, but could be --> <!-- Example: --> - <!-- <author email="jane.doe@example.com">Jane Doe</author> --> + <author email="johannes.mey@tu-dresden.de">Johannes Mey</author> <!-- The *depend tags are used to specify dependencies --> diff --git a/src/nng_receiver.cpp b/src/nng_receiver.cpp index a70fca3f11b75bdc2a1b9368fda233ff24e4fc82..ac094ddec9b98e67deaea4e2465a8d4058749481 100644 --- a/src/nng_receiver.cpp +++ b/src/nng_receiver.cpp @@ -3,31 +3,61 @@ // #include "ros/ros.h" -#include "std_msgs/String.h" #include <sstream> -int main(int argc, char **argv) -{ +#include <nng/nng.h> +#include <nng/protocol/pubsub0/sub.h> +#include <std_msgs/String.h> + +std::string url = "tcp://localhost:6576"; + +int main(int argc, char **argv) { ros::init(argc, argv, "nng_receiver"); ros::NodeHandle n("nnc_test"); - ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000); + ros::Publisher nng_receiver_pub = n.advertise<std_msgs::String>("received_nng", 1000); + + ros::Rate loop_rate(200); + + nng_socket sock; + int rv; + + if ((rv = nng_sub0_open(&sock)) != 0) { + ROS_ERROR_STREAM("nng_sub0_open returned: " << nng_strerror(rv)); + } + + // subscribe to everything (empty means all topics) + if ((rv = nng_setopt(sock, NNG_OPT_SUB_SUBSCRIBE, "", 0)) != 0) { + ROS_ERROR_STREAM("nng_setopt returned: " << nng_strerror(rv)); + } + if ((rv = nng_dial(sock, url.c_str(), nullptr, 0)) != 0) { + ROS_ERROR_STREAM("nng_dial returned: " << nng_strerror(rv)); + } - ros::Rate loop_rate(10); + while (ros::ok()) { - while (ros::ok()) - { - std_msgs::String msg; + char *buf = nullptr; + size_t sz; + if ((rv = nng_recv(sock, &buf, &sz, NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK)) == 0) { - std::stringstream ss; - ss << "nng message received: "; - msg.data = ss.str(); + // store the result in a string and free the buffer + std::string result(buf, sz); + nng_free(buf, sz); - ROS_INFO("%s", msg.data.c_str()); + ROS_INFO_STREAM("CLIENT: RECEIVED '" << result << "'"); - chatter_pub.publish(msg); + // build and publish a ROS message + std_msgs::String msg; + msg.data = result; + ROS_INFO_STREAM("Publishing ROS message '" << msg.data << "'"); + nng_receiver_pub.publish(msg); + } else if (rv == NNG_EAGAIN) { + // no message received in current spin + } else { + ROS_ERROR_STREAM("nng_recv returned: " << nng_strerror(rv)); + } ros::spinOnce(); diff --git a/src/nng_sender.cpp b/src/nng_sender.cpp index 74fddad93ae199549564c099adb3e73dfe9ae7be..afad3f14eadff4cc0a80d4349814acdee0c2950c 100644 --- a/src/nng_sender.cpp +++ b/src/nng_sender.cpp @@ -5,18 +5,34 @@ #include "ros/ros.h" #include "std_msgs/String.h" -void chatterCallback(const std_msgs::String::ConstPtr& msg) -{ - ROS_INFO("I heard: [%s]", msg->data.c_str()); +#include <nng/nng.h> +#include <nng/protocol/pubsub0/pub.h> + +std::string url = "tcp://localhost:6576"; + +nng_socket sock; +int rv; + +void nngCallback(const std_msgs::String::ConstPtr &msg) { + ROS_INFO("sending via NNG: [%s]", msg->data.c_str()); + if ((rv = nng_send(sock, (void *) msg->data.c_str(), msg->data.length(), 0)) != 0) { + ROS_ERROR_STREAM("nng_send returned: " << nng_strerror(rv)); + } } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "nng_sender"); ros::NodeHandle n("nnc_test"); - ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); + if ((rv = nng_pub0_open(&sock)) != 0) { + ROS_ERROR_STREAM("nng_pub0_open returned: " << nng_strerror(rv)); + } + if ((rv = nng_listen(sock, url.c_str(), nullptr, 0)) < 0) { + ROS_ERROR_STREAM("nng_listen returned: " << nng_strerror(rv)); + } + + ros::Subscriber sub = n.subscribe("send_nng", 1000, nngCallback); ros::spin();