Commits (7)
......@@ -125,6 +125,7 @@ catkin_package(
${PROJECT_NAME}_moveit
${PROJECT_NAME}_nng
${PROJECT_NAME}_mqtt
${PROJECT_NAME}_ros
${Protobuf_LIBRARIES}
CATKIN_DEPENDS
message_runtime
......@@ -170,6 +171,7 @@ add_library(${PROJECT_NAME}_base ${PROTO_SRCS} ${PROTO_HDRS} src/controller/Dumm
add_library(${PROJECT_NAME}_moveit ${PROTO_SRCS} ${PROTO_HDRS} src/controller/MoveItRobotArmController.cpp)
add_library(${PROJECT_NAME}_nng ${PROTO_SRCS} ${PROTO_HDRS} src/connection/NngConnection.cpp)
add_library(${PROJECT_NAME}_mqtt ${PROTO_SRCS} ${PROTO_HDRS} src/connection/MqttConnection.cpp)
add_library(${PROJECT_NAME}_ros ${PROTO_SRCS} ${PROTO_HDRS} src/connection/RosConnection.cpp)
add_library(${PROJECT_NAME}_scene_collision_object src/util/scene_collision_object.cpp)
## Add cmake target dependencies of the library
......@@ -180,6 +182,7 @@ add_dependencies(${PROJECT_NAME}_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${cat
add_dependencies(${PROJECT_NAME}_moveit ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_nng ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_mqtt ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_ros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_scene_collision_object ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
......@@ -199,6 +202,9 @@ target_link_libraries(${PROJECT_NAME}_nng
${catkin_LIBRARIES}
nng
)
target_link_libraries(${PROJECT_NAME}_ros
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_mqtt
${catkin_LIBRARIES}
paho-mqttpp3
......
//
// Created by Sebastian Ebert on 29.10.21.
//
#ifndef SRC_ROSCONNECTION_H
#define SRC_ROSCONNECTION_H
#include "Connection.h"
#include <thread>
#include <vector>
#include <ros/ros.h>
#include <map>
class RosConnection : public Connection {
std::vector<std::string> ros_topics;
std::unique_ptr<std::thread> ros_spinner_thread;
std::vector<ros::Subscriber> ros_subscribers;
std::map<std::string, ros::Publisher> ros_publishers;
ros::NodeHandle node_handle;
int input_queue_size;
int output_queue_size;
public:
/**
* A connection to the ROS based communication, limited to std_messages/String.
* @param n a valid node handle
* @param input_queue_size of the subscribers
* @param output_queue_size of the publishers
*/
explicit RosConnection(const ros::NodeHandle &n, int input_queue_size, int output_queue_size);
bool send(const std::string &channel, const std::string &message) override;
bool initializeConnection(std::function<void(std::string, std::string)> callback) override;
bool listen(const std::string &channel) override;
/**
* Grands the possibility to change the node handle and thus the sender namespace.
* @param n the node handel to be set
*/
void setHandle(const ros::NodeHandle &n);
/**
* Adds a publisher, to be used to publish messages to a certain topic.
* Needs to be done because there is delay between registering a publisher, and the time where one can use the publisher.
* @param channel the topic on which the publisher sends
* @return true if successfull added
*/
bool addPublisher(const std::string &channel);
/**
* Receive messages on all configured topics.
*/
void receiveMessages();
~RosConnection();
};
#endif //SRC_ROSCONNECTION_H
//
// Created by Sebastian Ebert on 29.10.21.
//
#include "ccf/connection/RosConnection.h"
#include <ros/ros.h>
#include <memory>
#include <std_msgs/String.h>
RosConnection::RosConnection(const ros::NodeHandle &n, int input_queue_size, int output_queue_size) : node_handle{n},
input_queue_size{input_queue_size}, output_queue_size{output_queue_size} {
}
void RosConnection::setHandle(const ros::NodeHandle &n) {
node_handle = n;
}
bool RosConnection::listen(const std::string &channel){
ros_topics.push_back(channel);
return true;
}
bool RosConnection::initializeConnection(std::function<void(std::string, std::string)> callback) {
messageReceivedCallback = callback;
ros_spinner_thread = std::make_unique<std::thread>(&RosConnection::receiveMessages, this);
return true;
}
bool RosConnection::addPublisher(const std::string &channel){
if(ros_publishers.count(channel) == 0){
ros::Publisher pub = node_handle.advertise<std_msgs::String>(channel, output_queue_size);
ros_publishers.insert(std::pair<std::string,ros::Publisher>(channel,pub));
return true;
}
return false;
}
bool RosConnection::send(const std::string &channel, const std::string &message) {
if (!channel.empty()) {
for (const auto& [key, value] : ros_publishers) {
if(key == channel){
std_msgs::String msg;
msg.data = message;
value.publish(msg);
return true;
}
}
}
return false;
}
void RosConnection::receiveMessages() {
for(std::string &topic : ros_topics){
ros::Subscriber s = node_handle.subscribe<std_msgs::String>(topic, input_queue_size,
[&](const std_msgs::StringConstPtr &msg) {
messageReceivedCallback(topic, msg->data);
});
ros_subscribers.push_back(s);
}
}
RosConnection::~RosConnection() {
ros_spinner_thread->join();
}