Skip to content
Snippets Groups Projects
Commit d60fe2c5 authored by Johannes Mey's avatar Johannes Mey
Browse files

transmission of nng message working

parent e058f906
No related branches found
No related tags found
No related merge requests found
...@@ -11,7 +11,11 @@ find_package(catkin REQUIRED roscpp) ...@@ -11,7 +11,11 @@ find_package(catkin REQUIRED roscpp)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # 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 ## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed ## modules and global scripts declared therein get installed
...@@ -100,7 +104,7 @@ find_package(nng CONFIG REQUIRED) ...@@ -100,7 +104,7 @@ find_package(nng CONFIG REQUIRED)
## DEPENDS: system dependencies of this project that dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package( catkin_package(
# INCLUDE_DIRS include # INCLUDE_DIRS include
# LIBRARIES nng_test LIBRARIES nng_test
# CATKIN_DEPENDS other_catkin_pkg # CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib # DEPENDS system_lib
) )
...@@ -146,9 +150,11 @@ set_target_properties(${PROJECT_NAME}_receiver PROPERTIES OUTPUT_NAME receiver P ...@@ -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 ## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_sender target_link_libraries(${PROJECT_NAME}_sender
${catkin_LIBRARIES} ${catkin_LIBRARIES}
nng
) )
target_link_libraries(${PROJECT_NAME}_receiver target_link_libraries(${PROJECT_NAME}_receiver
${catkin_LIBRARIES} ${catkin_LIBRARIES}
nng
) )
############# #############
......
...@@ -5,8 +5,6 @@ ...@@ -5,8 +5,6 @@
<description>a sender and receiver node using nng</description> <description>a sender and receiver node using nng</description>
<!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- 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> <maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer>
...@@ -25,7 +23,7 @@ ...@@ -25,7 +23,7 @@
<!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be --> <!-- Authors do not have to be maintainers, but could be -->
<!-- Example: --> <!-- 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 --> <!-- The *depend tags are used to specify dependencies -->
......
...@@ -3,31 +3,61 @@ ...@@ -3,31 +3,61 @@
// //
#include "ros/ros.h" #include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream> #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::init(argc, argv, "nng_receiver");
ros::NodeHandle n("nnc_test"); 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(10); ros::Rate loop_rate(200);
while (ros::ok()) nng_socket sock;
{ int rv;
std_msgs::String msg;
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));
}
while (ros::ok()) {
std::stringstream ss; char *buf = nullptr;
ss << "nng message received: "; size_t sz;
msg.data = ss.str(); if ((rv = nng_recv(sock, &buf, &sz, NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK)) == 0) {
ROS_INFO("%s", msg.data.c_str()); // store the result in a string and free the buffer
std::string result(buf, sz);
nng_free(buf, sz);
chatter_pub.publish(msg); ROS_INFO_STREAM("CLIENT: RECEIVED '" << result << "'");
// 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(); ros::spinOnce();
......
...@@ -5,18 +5,34 @@ ...@@ -5,18 +5,34 @@
#include "ros/ros.h" #include "ros/ros.h"
#include "std_msgs/String.h" #include "std_msgs/String.h"
void chatterCallback(const std_msgs::String::ConstPtr& msg) #include <nng/nng.h>
{ #include <nng/protocol/pubsub0/pub.h>
ROS_INFO("I heard: [%s]", msg->data.c_str());
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::init(argc, argv, "nng_sender");
ros::NodeHandle n("nnc_test"); 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(); ros::spin();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment