diff --git a/CMakeLists.txt b/CMakeLists.txt index 458dd4baafa7459724b7ca5e9f4a53a404667239..6ba1f887e94126089d27ece8e16535fb8aa81161 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,18 +1,6 @@ -#cmake_minimum_required(VERSION 3.10) -#project(MY_PROJECT) -#set(CMAKE_CXX_STANDARD 14) - -#add_executable(MY_PROJECT main.cpp) -# Perform linking of the paho lib -#target_link_libraries(MY_PROJECT LINK_PUBLIC ${PahoMqtt} ${PahoMqttCpp}) -# LINK_PUBLIC vs LINK_PRIVATE: https://cmake.org/cmake/help/v3.10/command/target_link_libraries.html - - - cmake_minimum_required(VERSION 2.8.3) project(panda_simulation) -# Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++14) set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}") @@ -41,6 +29,7 @@ find_package(catkin REQUIRED # System dependencies are found with CMake's conventions find_package(Boost REQUIRED COMPONENTS filesystem) +find_package(Protobuf 3 REQUIRED) find_library(PahoMqtt3c libpaho-mqtt3c.so REQUIRED) find_library(PahoMqtt3cs libpaho-mqtt3cs.so REQUIRED) @@ -70,7 +59,6 @@ catkin_package(CATKIN_DEPENDS hardware_interface pluginlib DEPENDS - #libpaho-mqtt3 # system_lib ) @@ -83,6 +71,7 @@ include_directories(${catkin_INCLUDE_DIRS}) # add custom controller as library add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp) +add_library(linkstate src/linkstate.pb.cc src/linkstate.pb.h) # Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES}) @@ -92,7 +81,7 @@ add_executable(box_publisher_node src/box_publisher.cpp) add_executable(robot_state_initializer_node src/robot_state_initializer.cpp) add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp) add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp) -add_executable(RobotStateProvider src/RobotStateProvider.cpp) +add_executable(RobotStateProvider src/RobotStateProvider.cpp src/mem_persistence.cpp) add_executable(TimedCartesianPlanner src/TimedCartesianPlanner.cpp) add_executable(MqttToRosNode src/MqttToRosNode.cpp) @@ -107,7 +96,9 @@ target_link_libraries(RobotStateProvider LINK_PUBLIC ${PahoMqtt3cs} ${PahoMqtt3a} ${PahoMqtt3as} - ${PahoMqttCpp}) + ${PahoMqttCpp} + ${Protobuf_LIBRARIES} + linkstate) target_link_libraries(MqttToRosNode LINK_PUBLIC ${catkin_LIBRARIES} ${PahoMqtt3c} diff --git a/proto/dataconfig.proto b/proto/dataconfig.proto new file mode 100644 index 0000000000000000000000000000000000000000..3ec464ef47c245a54b331d990dd137ff1eebe17c --- /dev/null +++ b/proto/dataconfig.proto @@ -0,0 +1,13 @@ +syntax = "proto2"; + +package config; + +message DataConfig { + + required bool enablePosition = 1; + required bool enableOrientation = 2; + required bool enableTwistLinear = 3; + required bool enableTwistAngular = 4; + + required int32 publishRate = 5; +} \ No newline at end of file diff --git a/proto/linkstate.proto b/proto/linkstate.proto new file mode 100644 index 0000000000000000000000000000000000000000..df7fd3a1c79959ca161a9ec83b42d4d96319b2c8 --- /dev/null +++ b/proto/linkstate.proto @@ -0,0 +1,38 @@ +syntax = "proto2"; + +package panda; + +message PandaLinkState { + + required string name = 1; + + message Position { + required float positionX = 1; + required float positionY = 2; + required float positionZ = 3; + } + + message Orientation { + required float orientationX = 1; + required float orientationY = 2; + required float orientationZ = 3; + required float orientationW = 4; + } + + message TwistLinear { + required float twistLinearX = 1; + required float twistLinearY = 2; + required float twistLinearZ = 3; + } + + message TwistAngular { + required float twistAngularX = 1; + required float twistAngularY = 2; + required float twistAngularZ = 3; + } + + optional Position pos = 2; + optional Orientation orient = 3; + optional TwistLinear tl = 4; + optional TwistAngular ta = 5; +} \ No newline at end of file diff --git a/proto/robotconfig.proto b/proto/robotconfig.proto new file mode 100644 index 0000000000000000000000000000000000000000..30dbbb75a3e8e8b3ae95afe2efb7397338d8bab6 --- /dev/null +++ b/proto/robotconfig.proto @@ -0,0 +1,8 @@ +syntax = "proto2"; + +package config; + +message RobotConfig { + + required double speed = 1; +} \ No newline at end of file diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp index b370c3c0d81b648fcd7281f7797ac93fbb079b0e..cb46a3bfd64ffbda53b2ea4f4bb5897d597195a9 100644 --- a/src/RobotStateProvider.cpp +++ b/src/RobotStateProvider.cpp @@ -5,6 +5,8 @@ #include <gazebo_msgs/LinkStates.h> #include <mqtt/client.h> #include "string.h" +#include "linkstate.pb.h" +#include "mem_persistence.cpp" // CONFIGURATION START // ^^^^^^^^^^^^^^^^^^^ @@ -17,130 +19,199 @@ bool export_position = true; bool export_orientation = true; bool export_twist_linear = true; bool export_twist_angular = true; -bool export_names = true; + +bool export_to_log = false; // CONFIGURATION END // ^^^^^^^^^^^^^^^^^ -const std::string SERVER_ADDRESS { "tcp://localhost:1883" }; -const std::string CLIENT_ID { "ros_mqtt_client" }; -const std::string TOPIC { "robot_data_feed" }; -const int QOS = 1; +/* + * mqtt-topics for all links + * ground, link0, link1, link2, link3, link4, link5, link6, link7 (end-effector), left finger, right finder + */ +std::string topics [11] = {"panda_link_0","panda_link_1","panda_link_2" ,"panda_link_3", + "panda_link_4","panda_link_5","panda_link_6","panda_link_7", + "panda_link_8","panda_link_9","panda_link_10"}; -int current_redirect = 0; +bool mqttSetup = false; bool isInitialized = false; +const int QOS = 1; +int current_redirect = 0; + +mem_persistence persist; +mqtt::client client("tcp://localhost:1883", "robot_state_provider_mqtt", &persist); + /* * logs to its own file in /.ros/logs (configured in launch-file) */ -void exportMessageToLog(const gazebo_msgs::LinkStates & msg) -{ - if(export_names){ - - ROS_INFO_STREAM("<<< LINK NAMES >>>"); +void exportMessageToLog(const gazebo_msgs::LinkStates &msg) { + ROS_INFO_STREAM("<<< LINK NAMES >>>"); - for(int i = 0; i < msg.name.size(); i++) - { - ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i)); - } + for (int i = 0; i < msg.name.size(); i++) { + ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i)); } - if(export_position){ + if (export_position) { ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>"); - for(int i = 0; i < msg.pose.size(); i++) - { + for (int i = 0; i < msg.pose.size(); i++) { ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x); ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y); ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z); } } - if(export_orientation){ + if (export_orientation) { ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>"); - for(int i = 0; i < msg.pose.size(); i++) - { - ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w); - ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x); - ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y); - ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z); + for (int i = 0; i < msg.pose.size(); i++) { + ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w); + ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x); + ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y); + ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z); } } - if(export_twist_linear){ + if (export_twist_linear) { ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>"); - for(int i = 0; i < msg.twist.size(); i++) - { - ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x); - ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y); - ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z); + for (int i = 0; i < msg.twist.size(); i++) { + ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x); + ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y); + ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z); } } - if(export_twist_angular){ + if (export_twist_angular) { ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>"); - for(int i = 0; i < msg.twist.size(); i++) - { - ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x); - ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y); - ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z); + for (int i = 0; i < msg.twist.size(); i++) { + ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x); + ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y); + ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z); } } } -void providerCallback(const gazebo_msgs::LinkStates & msg) -{ - if(current_redirect != 0 && current_redirect != message_redirect_rate){ - //std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl; +panda::PandaLinkState buildLinkStateMessage(const gazebo_msgs::LinkStates &msg, int link_number) { + + panda::PandaLinkState pls_msg; + + pls_msg.set_name(msg.name.at(link_number)); + + if (export_position) { + panda::PandaLinkState_Position *pos = new panda::PandaLinkState_Position(); + pos->set_positionx(msg.pose.at(link_number).position.x); + pos->set_positiony(msg.pose.at(link_number).position.y); + pos->set_positionz(msg.pose.at(link_number).position.z); + pls_msg.set_allocated_pos(pos); + } + + if (export_orientation) { + panda::PandaLinkState_Orientation *orient = new panda::PandaLinkState_Orientation(); + orient->set_orientationw(msg.pose.at(link_number).orientation.w); + orient->set_orientationx(msg.pose.at(link_number).orientation.x); + orient->set_orientationy(msg.pose.at(link_number).orientation.y); + orient->set_orientationz(msg.pose.at(link_number).orientation.z); + pls_msg.set_allocated_orient(orient); + } + + if (export_twist_linear) { + panda::PandaLinkState_TwistLinear *tl = new panda::PandaLinkState_TwistLinear(); + tl->set_twistlinearx(msg.twist.at(link_number).linear.x); + tl->set_twistlineary(msg.twist.at(link_number).linear.y); + tl->set_twistlinearz(msg.twist.at(link_number).linear.z); + pls_msg.set_allocated_tl(tl); + } + + if (export_twist_angular) { + panda::PandaLinkState_TwistAngular *ta = new panda::PandaLinkState_TwistAngular(); + ta->set_twistangularx(msg.twist.at(link_number).angular.x); + ta->set_twistangulary(msg.twist.at(link_number).angular.y); + ta->set_twistangularz(msg.twist.at(link_number).angular.z); + pls_msg.set_allocated_ta(ta); + } + + return pls_msg; +} + +void setupMqtt() { + + if (!mqttSetup) { + std::cout << "Initializing..." << std::endl; + + mqtt::connect_options connOpts; + connOpts.set_keep_alive_interval(20); + connOpts.set_clean_session(true); + std::cout << "...OK" << std::endl; + + std::cout << "\nConnecting..." << std::endl; + client.connect(connOpts); + std::cout << "...OK" << std::endl; + mqttSetup = true; + } +} + + +void sendMqttMessages(const gazebo_msgs::LinkStates &msg) { + + setupMqtt(); + + for(int i = 0; i < msg.name.size(); i++) + { + panda::PandaLinkState pls_msg = buildLinkStateMessage(msg, i); + std::string mqtt_msg; + pls_msg.SerializeToString(&mqtt_msg); + + //std::cout << "SENDING MESSAGE" << std::endl; + auto pubmsg = mqtt::make_message(topics[i], mqtt_msg); + pubmsg->set_qos(QOS); + client.publish(pubmsg); + } +} + +void providerCallback(const gazebo_msgs::LinkStates &msg) { + if (current_redirect != 0 && current_redirect != message_redirect_rate) { + // std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl; current_redirect++; return; } - if(current_redirect == 0 || current_redirect == message_redirect_rate){ + if (current_redirect == 0 || current_redirect == message_redirect_rate) { //std::cout << "Redirecting message:" << msg.pose.at(1).position << std::endl; - exportMessageToLog(msg); - if(current_redirect == message_redirect_rate){ + + if(export_to_log){ + exportMessageToLog(msg); + } + + sendMqttMessages(msg); + + if (current_redirect == message_redirect_rate) { current_redirect = 1; - } else{ + } else { current_redirect++; } } } -void sendMqttMessage(){ - mqtt::client client(SERVER_ADDRESS, CLIENT_ID); - - mqtt::connect_options connOpts; - connOpts.set_keep_alive_interval(20); - connOpts.set_clean_session(true); - - client.connect(connOpts); - auto pubmsg = mqtt::make_message(TOPIC, "test"); - pubmsg->set_qos(QOS); - client.publish(pubmsg); - - client.disconnect(); -} - -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "listener"); ros::NodeHandle n; n.getParam("ready", isInitialized); - // make sure the robot is initialized - while(!isInitialized){ - //std::cout << "Waiting" << std::endl; - n.getParam("tud_planner_ready", isInitialized); - } - - //sendMqttMessage(); + while (ros::ok()) { + // make sure the robot is initialized + while (!isInitialized) { + //std::cout << "Waiting" << std::endl; + n.getParam("tud_planner_ready", isInitialized); + } - ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback); - ros::spin(); + ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback); + ros::spin(); + } + // disconnect from mqtt + client.disconnect(); return 0; } \ No newline at end of file diff --git a/src/mem_persistence.cpp b/src/mem_persistence.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b6da0550a59c174145d2b831b4bcc978782b63fa --- /dev/null +++ b/src/mem_persistence.cpp @@ -0,0 +1,71 @@ +#include <string> +#include <map> +#include <vector> +#include "mqtt/client.h" + +// Connection persistence class. +class mem_persistence : virtual public mqtt::iclient_persistence { + // Whether the store is open + bool open_; + + // Use an STL map to store shared persistence pointers + // against string keys. + std::map<std::string, std::string> store_; + +public: + mem_persistence() : open_(false) {} + + // "Open" the store + void open(const std::string &clientId, const std::string &serverURI) override { + open_ = true; + } + + // Close the persistent store that was previously opened. + void close() override { + open_ = false; + } + + // Clears persistence, so that it no longer contains any persisted data. + void clear() override { + store_.clear(); + } + + // Returns whether or not data is persisted using the specified key. + bool contains_key(const std::string &key) override { + return store_.find(key) != store_.end(); + } + + // Returns the keys in this persistent data store. + const mqtt::string_collection &keys() const override { + static mqtt::string_collection ks; + ks.clear(); + for (const auto &k : store_) + ks.push_back(k.first); + return ks; + } + + // Puts the specified data into the persistent store. + void put(const std::string &key, const std::vector<mqtt::string_view> &bufs) override { + std::string str; + for (const auto &b : bufs) + str += b.str(); + store_[key] = std::move(str); + } + + // Gets the specified data out of the persistent store. + mqtt::string_view get(const std::string &key) const override { + auto p = store_.find(key); + if (p == store_.end()) + throw mqtt::persistence_exception(); + + return mqtt::string_view(p->second); + } + + // Remove the data for the specified key. + void remove(const std::string &key) override { + auto p = store_.find(key); + if (p == store_.end()) + throw mqtt::persistence_exception(); + store_.erase(p);; + } +}; \ No newline at end of file