├── src ├── libwrapper │ ├── config │ │ ├── data_types.h │ │ └── config.h │ ├── msg │ │ └── Info.msg │ ├── srv │ │ └── Manage.srv │ ├── include │ │ ├── compressor.h │ │ ├── ShapeShifterMessage.h │ │ ├── compressor.h.autosave │ │ ├── QOSTopicManager.h │ │ ├── TimeSpec.h │ │ ├── TFTopicManager.h │ │ ├── ShapeShifterListener.h │ │ ├── ShapeShifterSender.h │ │ ├── ServiceManager.h │ │ ├── argon.h │ │ ├── TopicManager.h │ │ ├── Manager.h │ │ └── ShapeShifterManager.h │ ├── package.xml │ ├── CMakeLists.txt │ ├── interface │ │ └── interface.h │ ├── compressor.cpp │ └── main.cpp ├── ros_pound │ ├── package.xml │ ├── readme.txt │ ├── CMakeLists.txt │ └── ros-pound.cpp └── utils │ ├── Argo_old.h │ └── Argo.h ├── README.md └── LICENSE /src/libwrapper/config/data_types.h: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/libwrapper/msg/Info.msg: -------------------------------------------------------------------------------- 1 | int32 serial 2 | int32 loop_id 3 | int8 connected 4 | int8[] lqm 5 | int8[] dist 6 | -------------------------------------------------------------------------------- /src/libwrapper/srv/Manage.srv: -------------------------------------------------------------------------------- 1 | string command 2 | string param1 3 | int32 param2 4 | --- 5 | string info 6 | int32 result 7 | -------------------------------------------------------------------------------- /src/libwrapper/config/config.h: -------------------------------------------------------------------------------- 1 | /** TOPIC / SERVICES DEFINITION 2 | * 3 | * TOPIC(type, topic, source, dest, priority, period, time_to_live) 4 | * QOSTOPIC(type, topic, source, dest, priority, period, time_to_live, queue_length) 5 | * TFTOPIC(topic, source, dest, priority, period, time_to_live) 6 | * 7 | * SERVICE(type, topic, source, priority, time_to_live) 8 | * 9 | */ 10 | 11 | -------------------------------------------------------------------------------- /src/libwrapper/include/compressor.h: -------------------------------------------------------------------------------- 1 | #ifndef CMP_H 2 | #define CMP_H 3 | 4 | int compressor_push(char * data, unsigned int size, unsigned char priority, unsigned char port, unsigned char src, unsigned int dst, unsigned int deadline); 5 | void * compressor_pop(int port, int timeout, char* & data, unsigned int & size, unsigned char & src, unsigned char & priority); 6 | int compressor_pop_done(void * data); 7 | int compressor_set_enabled(bool enabled); 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /src/ros_pound/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_pound 3 | ros-pound node 4 | Danilo Tardioli 5 | 1.1.0 6 | Danilo Tardioli, dantard@unizar.es 7 | GPL v2 8 | catkin 9 | 10 | message_generation 11 | roscpp 12 | std_msgs 13 | libpcap 14 | rospy 15 | tf 16 | roscpp_serialization 17 | libwrapper 18 | 19 | roscpp 20 | roscpp_serialization 21 | roscpp_tutorials 22 | libwrapper 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/libwrapper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | libwrapper 3 | libwrapper 4 | Danilo Tardioli 5 | 1.1.0 6 | Danilo Tardioli, dantard@unizar.es 7 | GPL v2 8 | catkin 9 | 10 | message_generation 11 | roscpp 12 | roscpp_serialization 13 | libpcap 14 | std_msgs 15 | topic_tools 16 | 17 | message_runtime 18 | roscpp 19 | roscpp_serialization 20 | std_msgs 21 | topic_tools 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/ros_pound/readme.txt: -------------------------------------------------------------------------------- 1 | Pound is a ROS package, which contains a ROS node to enalbe efficient multi-master ROS solution for wireless multi-robot networks in terms of reduced delay and reduced jitter. A configuration file to specify parameters of the Pound node should be added as ~/.rospound/config.yaml 2 | 3 | Example configuration file: 4 | 5 | nodes: 6 | - id: 3 7 | mac: 22:44:55:FF:00:01 8 | - id: 2 9 | mac: 18:19:20:21:22:23 10 | device: wlan0 11 | use_ip: true 12 | base_ip: 192.168.1.1 13 | routes: 14 | - dest: 3 15 | next: 5 16 | - dest: 4 17 | next: 5 18 | 19 | topics: 20 | - name: laser 21 | priority: 45 22 | period: 500 23 | source: 0 24 | dest: 0 25 | deadline: 45 26 | 27 | 28 | 29 | Topic definition (compile time) is as follows: 30 | 31 | TOPIC(type, topic, source, dest, priority, period, time_to_live) 32 | 33 | and services as: 34 | 35 | SERVICE(type, topic, source, priority, time_to_live) 36 | -------------------------------------------------------------------------------- /src/ros_pound/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | 3 | project(ros_pound) 4 | 5 | if(COMMAND cmake_policy) 6 | cmake_policy(SET CMP0003 NEW) 7 | cmake_policy(SET CMP0015 NEW) 8 | endif(COMMAND cmake_policy) 9 | 10 | find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs roscpp_serialization) 11 | find_package(Boost REQUIRED COMPONENTS thread) 12 | catkin_package(CATKIN_DEPENDS roscpp roscpp_serialization) 13 | 14 | include_directories(include ${catkin_INCLUDE_DIRS}) 15 | 16 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -Werror-implicit-function-declaration") 17 | SET(CMAKE_CXX_FLAGS "-std=c++0x -O3") 18 | 19 | add_executable (ros-pound 20 | ros-pound.cpp 21 | ) 22 | 23 | target_link_libraries(ros-pound wrapper z pthread rt pthread ${Boost_LIBRARIES} ${catkin_LIBRARIES} ) 24 | 25 | add_custom_command(TARGET ros-pound 26 | POST_BUILD 27 | COMMAND echo "Applying SGID bit to the executable..." 28 | COMMAND echo ${CMAKE_BINARY_DIR}/../devel/lib/ros_pound/ros-pound 29 | COMMAND sudo chmod a+rwx ${CMAKE_BINARY_DIR}/../devel/lib/ros_pound/ros-pound 30 | COMMAND sudo chown root:root ${CMAKE_BINARY_DIR}/../devel/lib/ros_pound/ros-pound 31 | COMMAND sudo chmod g+s ${CMAKE_BINARY_DIR}/../devel/lib/ros_pound/ros-pound 32 | ) 33 | -------------------------------------------------------------------------------- /src/libwrapper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | 3 | project(libwrapper) 4 | 5 | if(COMMAND cmake_policy) 6 | cmake_policy(SET CMP0003 NEW) 7 | cmake_policy(SET CMP0015 NEW) 8 | endif(COMMAND cmake_policy) 9 | 10 | find_package(catkin REQUIRED COMPONENTS message_generation roscpp roscpp_serialization std_msgs topic_tools) 11 | find_package(Boost REQUIRED COMPONENTS thread) 12 | 13 | add_message_files(FILES 14 | Info.msg 15 | ) 16 | 17 | add_service_files(FILES 18 | Manage.srv 19 | ) 20 | generate_messages() 21 | 22 | 23 | catkin_package(CATKIN_DEPENDS roscpp roscpp_serialization std_msgs topic_tools) 24 | include_directories(include ${catkin_INCLUDE_DIRS}) 25 | 26 | #set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build/bin) 27 | #set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build/lib) 28 | 29 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wextra -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -Werror-implicit-function-declaration") 30 | SET(CMAKE_CXX_FLAGS "-std=c++0x") 31 | 32 | add_library(wrapper 33 | main.cpp 34 | include/ShapeShifterManager.h 35 | include/ShapeShifterMessage.h 36 | include/ShapeShifterListener.h 37 | include/ShapeShifterSender.h 38 | include/TopicManager.h 39 | interface/interface.h 40 | include/ServiceManager.h 41 | include/TFTopicManager.h 42 | include/QOSTopicManager.h 43 | include/Manager.h 44 | include/argon.h 45 | include/TimeSpec.h 46 | include/compressor.h 47 | config/config.h 48 | config/data_types.h 49 | ) 50 | add_dependencies(wrapper ${PROJECT_NAME}_gencpp) 51 | 52 | target_link_libraries(wrapper z pthread rt pthread yaml-cpp bz2 ${Boost_LIBRARIES} ${catkin_LIBRARIES} ) 53 | -------------------------------------------------------------------------------- /src/libwrapper/include/ShapeShifterMessage.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #ifndef _SHAPESHITFER_MESSAGE_ 31 | #define _SHAPESHITFER_MESSAGE_ 32 | struct ShapeShifterMessage{ 33 | int size; 34 | unsigned short datatype_size; 35 | unsigned short msg_def_size; 36 | unsigned short topic_name_size; 37 | char data[0]; 38 | }; 39 | struct WrapperHeader{ 40 | unsigned int uncompressed_size; 41 | char data[0]; 42 | }; 43 | #endif 44 | -------------------------------------------------------------------------------- /src/libwrapper/include/compressor.h.autosave: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | #ifndef CMP_H 30 | #define CMP_H 31 | 32 | int compressor_push(char * data, unsigned int size, unsigned char priority, unsigned char port, unsigned char src, unsigned int dst, unsigned int deadline); 33 | void * compressor_pop(int port, int timeout, char* & data, unsigned int & size, unsigned char & src, unsigned char & priority); 34 | int compressor_pop_done(void * data); 35 | int compressor_set_enabled(bool enabled); 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /src/libwrapper/include/QOSTopicManager.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | #ifndef _QOS_TOPIC_MANAGER_ 30 | #define _QOS_TOPIC_MANAGER_ 31 | #include "TopicManager.h" 32 | 33 | template class QOSTopicManager: public TopicManager { 34 | int queue_length; 35 | 36 | public: 37 | QOSTopicManager(ros::NodeHandle & m, unsigned int port, std::string topic, unsigned char src, std::string dest, unsigned char priority, unsigned short deadline, unsigned short period, int queue_length){ 38 | this->init(m,port, topic,src,dest,priority,deadline, period); 39 | this->queue_length = queue_length; 40 | } 41 | 42 | virtual void listen() { 43 | T pm; 44 | bool started = false; 45 | while (ros::ok()) { 46 | int nelem = wrapper_get_num_of_enqueued_elements(this->port); 47 | if (nelem > queue_length){ 48 | started = true; 49 | } 50 | 51 | if (started && nelem > 0){ 52 | if (this->pop(pm)){ 53 | this->pub.publish(pm); 54 | } 55 | }else{ 56 | started = false; 57 | } 58 | usleep(this->period * 1000); 59 | } 60 | } 61 | }; 62 | #endif 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # unizar-pound-ros-pkg 2 | The Pound ROS node enables topic prioritization and supports multiple ROS cores in addition to the options of compression and configurations. It is mainly designed to improve communication performance (in terms of delay and jitter) in wireless multi-hop networks. 3 | 4 | Usage: 5 | `rosrun ros_pound ros-pound --node-id 0 --num-of-nodes 3` 6 | 7 | Parameter `node-id` specifies the identifier of the local node, parameter `num-of-nodes` specifies the total number of nodos in the robotic network 8 | 9 | By default, ros-pound uses UDP/IP communication. The network must have an address of the type `192.168.XX.YY`. XX is the network identificator while YY is related with the `node-id` parameter. Suppose the network is `192.168.1.YY`: 10 | 11 | * Node with `--node-id 0` *must* have IP `192.168.1.1` 12 | * Node with `--node-id 1` *must* have IP `192.168.1.2` 13 | 14 | etc. 15 | 16 | The base IP is by default `192.168.1.1`; this can be changed in the YAML configuration file specifying the parameter: 17 | 18 | `base_ip = 192.168.2.1` 19 | 20 | for example. 21 | 22 | The topics to be transported must be specified in the `config/config.h` file with the format: 23 | 24 | `TOPIC(type, topic_name, source, dest, priority, period, time_to_live)` 25 | 26 | where 27 | 28 | * `type` specifies the data type for example `std_msgs::Float64` 29 | * `topic_name` specifies the name of the topic, for example `"float_number"` 30 | * `source` is the source node of the topic, for example `0` 31 | * `dest` specifies the destination node(s) as text for example `"0,1,2"` however for the moment Pound only support unicast (only one node should be specified) 32 | * `priority` specifies the priority associated to the topic for example `56`. Must be between 0 and 127 33 | * `period` the expected period of the topic in ms, for example `100` 34 | * `time_to_live` period during the which the message is considered valid in ms, for example `500` 35 | 36 | The data type used must be specified in the `config/data_types.h` file. For example: 37 | 38 | `#include ` 39 | 40 | 41 | The complete list of options for the YAML file is: 42 | 43 | ``` 44 | use_ip: true 45 | device: wlan0 46 | base_ip: 192.168.1.1 47 | nodes: 48 | - id: 3 49 | mac: 22:44:55:FF:00:01 50 | - id: 2 51 | mac: 18:19:20:21:22:23 52 | routes: 53 | - dest: 3 54 | next: 5 55 | - dest: 4 56 | next: 5 57 | feedback: false 58 | auto_tuning: false 59 | port: 32000 60 | queue: 50 61 | delay: 2500 62 | rate_mbps: 6.0 63 | use_discard: false 64 | quiet: false 65 | ``` 66 | 67 | The Pound node is similar to RT-WMP package available at http://wiki.ros.org/ros-rt-wmp. While the RT-WMP assigns global priorities, the Pound assigns local (node or machine level) priorities. 68 | For more information, please contact dantard@unizar.es 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /src/libwrapper/interface/interface.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #include 31 | #include 32 | 33 | #define ROSWMP_DEBUG(output,...) // fprintf(output, __VA_ARGS__); fprintf(output,"\n"); 34 | 35 | 36 | int wrapper_get_node_id(); 37 | 38 | int wrapper_push(char * data, unsigned int size, unsigned char priority, unsigned char port, unsigned char src, unsigned int dst, unsigned int deadline); 39 | 40 | void * wrapper_pop(int port, int timeout, char* & data, unsigned int & size, unsigned char & src, unsigned char & priority); 41 | 42 | int wrapper_pop_done(void * data); 43 | 44 | /* needed by QoS topics */ 45 | int wrapper_get_num_of_enqueued_elements(int port); 46 | 47 | int wrapper_init(ros::NodeHandle & n, int node_id, int num_of_nodes, int num_of_ports); 48 | 49 | void wrapper_run_bg(); 50 | 51 | int wrapper_flow_add(unsigned int port, unsigned int period, unsigned short priority, unsigned int deadline); 52 | 53 | int wrapper_set_period(int port, int period); 54 | int wrapper_set_priority(int port, int priority); 55 | bool wrapper_call_service(std::string command, std::string param1, int param2, std::string & info, int &result); 56 | 57 | std::string wrapper_get_config_filename(); 58 | void wrapper_setup_config(YAML::Node, int _node_id, int _num_of_nodes, int _num_of_ports); 59 | int wrapper_main(int argc, char * argv[]); 60 | -------------------------------------------------------------------------------- /src/libwrapper/compressor.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include "interface/interface.h" 6 | #include "ShapeShifterMessage.h" 7 | 8 | struct cmp_header_t{ 9 | unsigned int uncmp_size; 10 | char data[0]; 11 | }; 12 | 13 | static int m_enabled = 0; 14 | 15 | 16 | int compressor_set_enabled(bool enabled){ 17 | m_enabled = enabled; 18 | } 19 | int compressor_push(char * data, unsigned int size, unsigned char priority, unsigned char port, unsigned char src, unsigned int dst, unsigned int deadline){ 20 | 21 | WrapperHeader * wh = (WrapperHeader *) data; 22 | wh->uncompressed_size = 0; 23 | 24 | if (m_enabled){ 25 | unsigned int payload = size - sizeof(WrapperHeader); 26 | unsigned int cmp_size = payload; 27 | 28 | char * buff = new char [cmp_size]; 29 | int bz_result = BZ2_bzBuffToBuffCompress(buff, &cmp_size, &wh->data[0], payload, 1, 0, 30); 30 | 31 | if (bz_result == BZ_OUTBUFF_FULL){ 32 | ROS_INFO_ONCE("Port %d -> negative compression, sending uncompressed", port); 33 | }else if (bz_result != 0){ 34 | ROS_WARN_THROTTLE(1000,"Port %d -> compression error (%d), sending uncompressed", port, bz_result); 35 | }else{ 36 | wh->uncompressed_size = payload; 37 | memcpy(&wh->data[0], buff, cmp_size); 38 | size = cmp_size + sizeof(WrapperHeader); 39 | } 40 | delete [] buff; 41 | } 42 | return wrapper_push(data, size, priority, port, src,dst,deadline); 43 | } 44 | 45 | void * compressor_pop(int port, int timeout, char* & data, unsigned int & size, unsigned char & src, unsigned char & priority){ 46 | 47 | 48 | std::pair * pnt = new std::pair(); 49 | pnt->first = wrapper_pop(port, timeout, data, size, src, priority); 50 | pnt->second = 0; 51 | 52 | WrapperHeader * wh1 = (WrapperHeader *) data; 53 | 54 | if (wh1->uncompressed_size >0){ 55 | WrapperHeader * wh2 = (WrapperHeader *) new char[wh1->uncompressed_size + sizeof(WrapperHeader)]; 56 | int bz_result = BZ2_bzBuffToBuffDecompress(&wh2->data[0], &wh1->uncompressed_size, &wh1->data[0], size - sizeof(WrapperHeader), 0, 0); 57 | if (bz_result != 0){ 58 | ROS_ERROR_THROTTLE(1000,"Port %d -> decompression error (%d), sending uncompressed", port, bz_result); 59 | pnt->first = 0; 60 | }else{ 61 | data = pnt->second = (char *) wh2; 62 | size = wh1->uncompressed_size; 63 | memcpy(wh2, wh1, sizeof(WrapperHeader)); 64 | } 65 | } 66 | return pnt; 67 | } 68 | 69 | int compressor_pop_done(void * data){ 70 | std::pair * pnt = (std::pair *) data; 71 | if (pnt->second){ 72 | delete pnt->second; 73 | } 74 | return wrapper_pop_done(pnt->first); 75 | } 76 | 77 | -------------------------------------------------------------------------------- /src/libwrapper/include/TimeSpec.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | class TimeSpec{ 31 | public: 32 | static void addms(struct timespec *ts, long ms) { 33 | int sec = ms / 1000; 34 | ms = ms - sec * 1000; 35 | ts->tv_nsec += ms * 1000000; 36 | ts->tv_sec += ts->tv_nsec / 1000000000 + sec; 37 | ts->tv_nsec = ts->tv_nsec % 1000000000; 38 | } 39 | 40 | static void future(struct timespec *ts, unsigned int ms) { 41 | now(ts); 42 | addms(ts,ms); 43 | } 44 | 45 | static void subtract(struct timespec *a, struct timespec *b) { 46 | a->tv_nsec = a->tv_nsec - b->tv_nsec; 47 | if (a->tv_nsec < 0) { 48 | // borrow. 49 | a->tv_nsec += 1000000000; 50 | a->tv_sec--; 51 | } 52 | a->tv_sec = a->tv_sec - b->tv_sec; 53 | } 54 | 55 | static int milliseconds(struct timespec *a) { 56 | return a->tv_sec * 1000 + a->tv_nsec / 1000000; 57 | } 58 | 59 | static int microseconds(struct timespec *a) { 60 | return a->tv_sec * 1000000 + a->tv_nsec / 1000; 61 | } 62 | 63 | static void now(struct timespec *ts) { 64 | clock_gettime(CLOCK_REALTIME, ts); 65 | } 66 | 67 | static unsigned long long timestamp() { 68 | struct timespec ts; 69 | now(&ts); 70 | return ((unsigned long long)(ts.tv_sec)) * 1000000 + ((unsigned long long)(ts.tv_nsec)) / 1000; 71 | } 72 | 73 | static unsigned int timestamp_ms() { 74 | struct timespec ts; 75 | now(&ts); 76 | unsigned long long res = ((unsigned long long)(ts.tv_sec)) * 1000000 + ((unsigned long long)(ts.tv_nsec)) / 1000; 77 | return (unsigned int)(res/1000); 78 | } 79 | 80 | static int subtract_to_ms(struct timespec *a, struct timespec *b) { 81 | subtract(a, b); 82 | return milliseconds(a); 83 | } 84 | static int elapsed_ms(struct timespec *a) { 85 | struct timespec ts; 86 | now(&ts); 87 | subtract(&ts, a); 88 | return milliseconds(&ts); 89 | } 90 | 91 | static int elapsed_us(struct timespec *a) { 92 | struct timespec ts; 93 | now(&ts); 94 | subtract(&ts, a); 95 | return microseconds(&ts); 96 | } 97 | 98 | static void elapsed_print_us(struct timespec *a, char * text){ 99 | fprintf(stderr, "%s: %d\n", text, elapsed_us(a)); 100 | } 101 | 102 | }; 103 | -------------------------------------------------------------------------------- /src/libwrapper/include/TFTopicManager.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #ifndef _TFTOPIC_MANAGER_ 31 | #define _TFTOPIC_MANAGER_ 32 | 33 | #include 34 | 35 | #include 36 | #include "TopicManager.h" 37 | 38 | class TFTopicManager : public TopicManager { 39 | 40 | std::vector black_list; 41 | std::vector white_list; 42 | std::string out_topic; 43 | tf2_msgs::TFMessage msg; 44 | boost::shared_ptr msg_ptr; 45 | 46 | struct timespec ts; 47 | public: 48 | TFTopicManager(ros::NodeHandle & m, unsigned int port, std::string topic, unsigned char src, std::string dest, unsigned char priority, unsigned short deadline, unsigned short period){ 49 | init(m, port, topic,src,dest,priority,deadline, period); 50 | TimeSpec::now(&ts); 51 | out_topic = topic; 52 | msg_ptr.reset(&msg); 53 | } 54 | 55 | std::map map; 56 | 57 | virtual std::string get_in_topic(){ 58 | return topic; 59 | } 60 | void set_out_topic(std::string name){ 61 | out_topic = name; 62 | } 63 | virtual std::string get_out_topic(){ 64 | return out_topic; 65 | } 66 | 67 | virtual void callback(const boost::shared_ptr & message) { 68 | for (int i = 0; itransforms.size(); i++){ 69 | geometry_msgs::TransformStamped ts = message->transforms.at(i); 70 | std::ostringstream oss; 71 | oss << ts.header.frame_id << "->" << ts.child_frame_id; 72 | 73 | if (black_list.size() > 0){ 74 | if (std::find(black_list.begin(), black_list.end(), oss.str()) != black_list.end()){ 75 | continue; 76 | } 77 | } 78 | 79 | if (white_list.size() > 0){ 80 | if (std::find(white_list.begin(), white_list.end(), oss.str()) == white_list.end()){ 81 | continue; 82 | } 83 | } 84 | 85 | map[oss.str()] = ts; 86 | } 87 | if (TimeSpec::elapsed_ms(&ts) > period){ 88 | std::map::iterator it; 89 | msg.transforms.clear(); 90 | for (it = map.begin(); it!=map.end(); it++){ 91 | msg.transforms.push_back(it->second); 92 | } 93 | push(msg_ptr); 94 | TimeSpec::now(&ts); 95 | } 96 | } 97 | 98 | void add_blacklist(std::string elem){ 99 | black_list.push_back(elem); 100 | } 101 | 102 | void add_whitelist(std::string elem){ 103 | white_list.push_back(elem); 104 | } 105 | }; 106 | #endif 107 | -------------------------------------------------------------------------------- /src/libwrapper/include/ShapeShifterListener.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #ifndef _SHAPESHITFER_LISTENER_ 31 | #define _SHAPESHITFER_LISTENER_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include "../interface/interface.h" 39 | #include "Manager.h" 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include "ShapeShifterMessage.h" 45 | 46 | class ShapeShifterListener: public Manager { 47 | protected: 48 | ros::Subscriber sub; 49 | ros::Publisher pub; 50 | bool started; 51 | public: 52 | 53 | ShapeShifterListener(ros::NodeHandle & m, unsigned int port){ 54 | init(m, port); 55 | } 56 | ShapeShifterListener(){} 57 | 58 | virtual void init(ros::NodeHandle & m, unsigned int port){ 59 | this->n = m; 60 | this->port = port; 61 | started = true; 62 | type = SS_LISTENER; 63 | } 64 | 65 | 66 | virtual std::string get_in_topic(){ 67 | std::ostringstream s; 68 | s << n.getNamespace() << "/tx/" << topic; 69 | return s.str(); 70 | } 71 | 72 | virtual std::string get_out_topic(std::string topic, int sender){ 73 | std::ostringstream s; 74 | s << n.getNamespace() << "/rx/R" << (int) sender << "/" << topic; 75 | return s.str(); 76 | } 77 | 78 | void start(){ 79 | started = true; 80 | } 81 | 82 | 83 | void stop(){ 84 | started = false; 85 | } 86 | 87 | virtual void run() { 88 | boost::thread(boost::bind(&ShapeShifterListener::listen, this)); 89 | } 90 | 91 | virtual bool pop(boost::shared_ptr & shapeShifter, std::string & name, int & sender){ 92 | unsigned int size; 93 | unsigned char src, priority; 94 | char * data; 95 | void * e = bz_pop(port,0, data, size, src,priority); 96 | 97 | sender = src; 98 | 99 | WrapperHeader * wh = (WrapperHeader *) data; 100 | ShapeShifterMessage * ssm = (ShapeShifterMessage *) &wh->data[0]; 101 | ros::serialization::IStream istream((uint8_t *)&ssm->data[0], ssm->size); 102 | 103 | std::string metadata(&ssm->data[0] + ssm->size); 104 | shapeShifter->morph(metadata.substr(0,32), metadata.substr(32, ssm->datatype_size), metadata.substr(32 + ssm->datatype_size, ssm->msg_def_size), "false"); 105 | shapeShifter->read(istream); 106 | 107 | name = metadata.substr(32 + ssm->datatype_size + ssm->msg_def_size, ssm->topic_name_size); 108 | 109 | if (e == NULL){ 110 | ROS_WARN_ONCE("Fuction pop must NOT return null for the data to be used"); 111 | return false; 112 | } 113 | 114 | bz_pop_done(e); 115 | return true; 116 | } 117 | 118 | struct publisher_t{ 119 | ros::Publisher pub; 120 | std::string md5, message_def, type; 121 | }; 122 | 123 | std::map publishers; 124 | 125 | virtual void listen() { 126 | boost::shared_ptr shapeShifter(new topic_tools::ShapeShifter); 127 | std::string name; 128 | int sender; 129 | while (ros::ok()) { 130 | if (pop(shapeShifter, name, sender)){ 131 | std::cerr << "popped on PORT " << port << get_out_topic(name, sender) << std::endl; 132 | 133 | if (publishers.find(name) == publishers.end()){ 134 | publishers[name].pub = shapeShifter->advertise(n,get_out_topic(name, sender),1); 135 | publishers[name].md5 = shapeShifter->getMD5Sum(); 136 | publishers[name].message_def = shapeShifter->getMessageDefinition(); 137 | publishers[name].type = shapeShifter->getDataType(); 138 | } 139 | publishers[name].pub.publish(shapeShifter); 140 | } 141 | } 142 | } 143 | }; 144 | #endif 145 | -------------------------------------------------------------------------------- /src/libwrapper/include/ShapeShifterSender.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #ifndef _SHAPESHITFER_SENDER_ 31 | #define _SHAPESHITFER_SENDER_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include "../interface/interface.h" 39 | #include "Manager.h" 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include "ShapeShifterMessage.h" 45 | #include 46 | 47 | class ShapeShifterSender: public Manager { 48 | protected: 49 | ros::Subscriber sub; 50 | ros::Publisher pub; 51 | unsigned int dest; 52 | bool started; 53 | std::string dest_str; 54 | public: 55 | 56 | ShapeShifterSender(ros::NodeHandle & m, unsigned int port, std::string topic, std::string dest, unsigned char priority, unsigned short deadline, unsigned short period){ 57 | init(m, port, topic,dest,priority,deadline, period); 58 | } 59 | ShapeShifterSender(){} 60 | 61 | virtual void init(ros::NodeHandle & m, unsigned int port, std::string topic, std::string dest, unsigned char priority, unsigned short deadline, unsigned short period){ 62 | this->n = m; 63 | this->port = port; 64 | this->priority = priority; 65 | this->deadline = deadline; 66 | this->period = period; 67 | this->topic = topic; 68 | this->type = TOPIC; 69 | this->dest_str = dest; 70 | parse_ids(dest_str, dests); 71 | this->dest = compute_broadcast_destination(dests); 72 | started = true; 73 | } 74 | 75 | virtual std::string get_in_topic(){ 76 | std::ostringstream s; 77 | s << n.getNamespace() << "/tx/" << topic; 78 | return s.str(); 79 | } 80 | 81 | void start(){ 82 | started = true; 83 | } 84 | 85 | void stop(){ 86 | started = false; 87 | } 88 | 89 | virtual void run() { 90 | wrapper_flow_add(port, period, priority, deadline); 91 | sub = n.subscribe(get_in_topic(), 1000, &ShapeShifterSender::callback, this); 92 | } 93 | 94 | std::vector ticks; 95 | int count = 0; 96 | 97 | bool is_time_to_push(){ 98 | 99 | ticks.push_back(TimeSpec::timestamp_ms()); 100 | if (ticks.size() > 5){ 101 | ticks.erase(ticks.begin()); 102 | } 103 | 104 | unsigned int sum = 0; 105 | for (int i = 0; i< ticks.size()-1; i++){ 106 | sum += (ticks[i+1] - ticks[i]); 107 | } 108 | 109 | int period = ticks.size() > 1 ? sum/(ticks.size()-1): 0; 110 | if (period == 0 || this->period == 0){ 111 | return true; 112 | } 113 | 114 | int rate = int(nearbyint(double(this->period)/double(period))); 115 | int real_period = rate*period; 116 | double error = fabs(1.0-(double(real_period)/double(this->period))); 117 | if (error > 0.05){ 118 | ROS_WARN_ONCE("Topic '%s' real period is %dms", topic.c_str(), real_period); 119 | } 120 | if (++count > rate-1){ 121 | count = 0; 122 | return true; 123 | } 124 | return false; 125 | } 126 | 127 | virtual void callback(const topic_tools::ShapeShifter::ConstPtr& message) { 128 | if (is_time_to_push()){ 129 | push(message); 130 | } 131 | } 132 | 133 | virtual void push(const topic_tools::ShapeShifter::ConstPtr& msg){ 134 | if (started){ 135 | std::ostringstream oss, aux; 136 | 137 | /* Remove comments from message definitions */ 138 | std::string md = boost::regex_replace(msg->getMessageDefinition(), boost::regex("[' ']{2,}"), " "); 139 | std::istringstream f(md); 140 | std::string line; 141 | while (std::getline(f, line)) { 142 | int pos = line.find("#"); 143 | if (pos == std::string::npos){ 144 | aux << line << std::endl; 145 | }else if (pos != 0){ 146 | aux << line.substr(0,pos - 1) << std::endl; 147 | } 148 | } 149 | /* done */ 150 | 151 | oss << msg->getMD5Sum() << msg->getDataType() << aux.str() /*msg->getMessageDefinition()*/ << topic; 152 | std::vector buffer(sizeof(WrapperHeader) + sizeof(ShapeShifterMessage) + msg->size() + oss.str().size()); 153 | 154 | WrapperHeader * wh = (WrapperHeader *) &buffer[0]; 155 | ShapeShifterMessage * ssm = (ShapeShifterMessage *) &wh->data[0]; 156 | 157 | ssm->size = msg->size(); 158 | ssm->datatype_size = msg->getDataType().size(); 159 | ssm->msg_def_size = aux.str().size(); //msg->getMessageDefinition().size(); 160 | ssm->topic_name_size = topic.size(); 161 | 162 | ros::serialization::OStream ostream((uint8_t *) &ssm->data[0], msg->size()); 163 | msg->write(ostream); 164 | memcpy(&ssm->data[0] + msg->size(), oss.str().c_str(), oss.str().size()); 165 | 166 | bz_push((char *) &buffer[0], buffer.size(), priority, port, wrapper_get_node_id(), dest, deadline); 167 | } 168 | } 169 | }; 170 | #endif 171 | -------------------------------------------------------------------------------- /src/libwrapper/include/ServiceManager.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #ifndef _SERVICE_MANAGER_ 31 | #define _SERVICE_MANAGER_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | 38 | #define SET_BIT(array, bit) (array |= (1 << bit)) 39 | 40 | #include 41 | #include "../interface/interface.h" 42 | #include "Manager.h" 43 | 44 | template class ServiceManager : public Manager { 45 | ros::ServiceServer service; 46 | bool im_server; 47 | unsigned short serial; 48 | 49 | struct service_data_t{ 50 | bool response; 51 | bool response2; 52 | unsigned short serial; 53 | char data[0]; 54 | }; 55 | 56 | public: 57 | ServiceManager(ros::NodeHandle & m, unsigned int port, std::string topic, unsigned char server, unsigned char priority, unsigned short deadline){ 58 | this->n = m; 59 | this->port = port; 60 | this->priority = priority; 61 | this->deadline = deadline; 62 | this->topic = topic; 63 | this->server = server; 64 | type = SERVICE; 65 | } 66 | 67 | virtual void run() { 68 | 69 | im_server = server == wrapper_get_node_id(); 70 | wrapper_flow_add(port, 0, priority, deadline); 71 | wrapper_flow_add(port+1, 0, priority, deadline); 72 | 73 | if (im_server) { 74 | boost::thread(boost::bind(&ServiceManager::listen, this)); 75 | }else{ 76 | std::ostringstream srv_topic; 77 | srv_topic << n.getNamespace() <<"/REM/R" << (int) server << "/" << topic; 78 | service = n.advertiseService(srv_topic.str(), &ServiceManager::callback, this); 79 | } 80 | } 81 | 82 | 83 | bool callback(typename T::Request &req, typename T::Response &resp) { 84 | 85 | ros::SerializedMessage buff = ros::serialization::serializeMessage(req); 86 | char message[sizeof(service_data_t) + buff.num_bytes]; 87 | 88 | /* Step 1: I receive a service call and send the request to the server node */ 89 | service_data_t * sd = (service_data_t *) message; 90 | memcpy(sd->data, buff.message_start, buff.num_bytes); 91 | sd->serial = serial ++; 92 | 93 | wrapper_push(message, sizeof(service_data_t) + buff.num_bytes, priority, port, wrapper_get_node_id(), 1 << server, deadline); 94 | 95 | /* Step 3: I wait for the answer of the server node */ 96 | bool successful = false; 97 | unsigned short recvd_serial = 0; 98 | do { 99 | char * data; unsigned int size; unsigned char src; unsigned char prio; 100 | void * e = wrapper_pop(port + 1, deadline*3, data, size, src, prio); 101 | if (e == NULL){ 102 | std::cerr << "timeout" << std::endl; 103 | return false; 104 | } 105 | 106 | sd = (service_data_t *) data; 107 | size = size - sizeof(service_data_t); 108 | 109 | typename T::Response response; 110 | if (!deserialize(sd->data, size, response)) { 111 | wrapper_pop_done(e); 112 | return false; 113 | } 114 | 115 | resp = response; 116 | successful = sd->response; 117 | recvd_serial = sd->serial; 118 | 119 | wrapper_pop_done(e); 120 | 121 | } while (serial != (recvd_serial + 1)); 122 | 123 | return successful; 124 | } 125 | 126 | virtual void listen() { 127 | 128 | typename T::Request request; 129 | ros::ServiceClient client = n.serviceClient (topic); 130 | while (ros::ok()) { 131 | unsigned int size; 132 | unsigned char src, priority; 133 | char * data; 134 | 135 | /* Step 2: I receive a service execution request*/ 136 | 137 | void * e = wrapper_pop(port, 0, data, size, src, priority); 138 | if (e == NULL){ 139 | continue; 140 | } 141 | 142 | service_data_t * sd = (service_data_t *) data; 143 | size = size - sizeof(service_data_t); 144 | int serial = sd->serial; 145 | 146 | if (!deserialize(sd->data, size, request)) { 147 | wrapper_pop_done(e); 148 | return; 149 | } 150 | wrapper_pop_done(e); 151 | 152 | /* Step 3: I execute the call and send the result back */ 153 | T call; 154 | call.request = request; 155 | bool response = client.call(call); 156 | 157 | 158 | ros::SerializedMessage buff = ros::serialization::serializeMessage(call.response); 159 | 160 | char message[sizeof(service_data_t) + buff.num_bytes]; 161 | sd = (service_data_t *) message; 162 | 163 | sd->serial = serial; 164 | sd->response = response; 165 | memcpy(sd->data, buff.message_start, buff.num_bytes); 166 | 167 | unsigned int dest = 0; 168 | SET_BIT(dest, src); 169 | 170 | wrapper_push(message, sizeof(service_data_t) + buff.num_bytes, priority, port + 1, wrapper_get_node_id(), dest , deadline); 171 | 172 | } 173 | } 174 | 175 | }; 176 | #endif 177 | -------------------------------------------------------------------------------- /src/libwrapper/include/argon.h: -------------------------------------------------------------------------------- 1 | /*------------------------------------------------------------------------ 2 | *--------------------- ARGON -------------------- 3 | *------------------------------------------------------------------------ 4 | * V0.1B 15/09/10 5 | * 6 | * 7 | * File: argon.h 8 | * Authors: Danilo Tardioli 9 | * ---------------------------------------------------------------------- 10 | * Copyright (C) 2000-2010, Universidad de Zaragoza, SPAIN 11 | * 12 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 13 | * 14 | * RT-WMP is free software; you can redistribute it and/or modify it 15 | * under the terms of the GNU General Public License as published by the 16 | * Free Software Foundation; either version 2, or (at your option) any 17 | * later version. 18 | * 19 | * RT-WMP is distributed in the hope that it will be useful, but 20 | * WITHOUT ANY WARRANTY; without even the implied warranty of 21 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 22 | * General Public License for more details. 23 | * 24 | * You should have received a copy of the GNU General Public License 25 | * distributed with RT-WMP; see file COPYING. If not, write to the 26 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 27 | * 02111-1307, USA. 28 | * 29 | * As a special exception, if you link this unit with other files to 30 | * produce an executable, this unit does not by itself cause the 31 | * resulting executable to be covered by the GNU General Public License. 32 | * This exception does not however invalidate any other reasons why the 33 | * executable file might be covered by the GNU Public License. 34 | * 35 | *----------------------------------------------------------------------*/ 36 | 37 | #ifndef ARGON_H_ 38 | #define ARGON_H_ 39 | 40 | #define ARGO_MAX_SWITCH 10 41 | 42 | int argo_addInt(int * var, char* sw, int dfl, int need_value); 43 | int argo_addDouble(double * var, char* sw, double dfl); 44 | int argo_addString(char * var, char* sw, char * dfl); 45 | void argo_setComment(char * sw, char * text); 46 | void argo_setCommentId(int id, char * text); 47 | void argo_doProcess(int argc, char * argv[], int start); 48 | 49 | /* CODE >>>>>>> */ 50 | 51 | #define ARGON_INT 0 52 | #define ARGON_STRING 1 53 | #define ARGON_DOUBLE 2 54 | #define WMP_ERROR fprintf 55 | #define STR strdup 56 | 57 | typedef struct { 58 | char sw[16]; 59 | char type; 60 | void * var; 61 | int need_value; 62 | int mandatory; 63 | int set; 64 | char comment[64]; 65 | } argo_data_t; 66 | 67 | static struct { 68 | argo_data_t at[ARGO_MAX_SWITCH]; 69 | char idx; 70 | } regs; 71 | 72 | static char example[256]; 73 | 74 | static void init(){ 75 | static int initied = 0; 76 | if (!initied){ 77 | memset(®s,0,sizeof(regs)); 78 | initied = 1; 79 | example[0] = 0; 80 | } 81 | 82 | } 83 | 84 | int argo_addInt(int * var, char* sw, int dfl, int need_value) { 85 | init(); 86 | *var = dfl; 87 | regs.at[(int)regs.idx].var = (void*) var; 88 | strcpy(regs.at[(int)regs.idx].sw, sw); 89 | regs.at[(int)regs.idx].type = ARGON_INT; 90 | regs.at[(int)regs.idx].need_value = need_value; 91 | sprintf(regs.at[(int)regs.idx].comment, "--%s: type int", sw); 92 | regs.idx++; 93 | return regs.idx - 1; 94 | } 95 | int argo_addIntMandatory(int * var, char* sw, int dfl, int need_value) { 96 | init(); 97 | *var = dfl; 98 | regs.at[(int)regs.idx].var = (void*) var; 99 | strcpy(regs.at[(int)regs.idx].sw, sw); 100 | regs.at[(int)regs.idx].type = ARGON_INT; 101 | regs.at[(int)regs.idx].need_value = need_value; 102 | regs.at[(int)regs.idx].mandatory = 1; 103 | sprintf(regs.at[(int)regs.idx].comment, "--%s: type int", sw); 104 | regs.idx++; 105 | return regs.idx - 1; 106 | } 107 | int argo_addDouble(double * var, char* sw, double dfl) { 108 | init(); 109 | *var = dfl; 110 | regs.at[(int)regs.idx].var = (double*) var; 111 | strcpy(regs.at[(int)regs.idx].sw, sw); 112 | regs.at[(int)regs.idx].type = ARGON_DOUBLE; 113 | sprintf(regs.at[(int)regs.idx].comment, "--%s: type double", sw); 114 | regs.idx++; 115 | return regs.idx - 1; 116 | } 117 | 118 | int argo_addString(char * var, char* sw, char * dfl) { 119 | init(); 120 | strcpy(var, dfl); 121 | regs.at[(int)regs.idx].var = (char*) var; 122 | strcpy(regs.at[(int)regs.idx].sw, sw); 123 | regs.at[(int)regs.idx].type = ARGON_STRING; 124 | sprintf(regs.at[(int)regs.idx].comment, "--%s: type string", sw); 125 | regs.idx++; 126 | return regs.idx - 1; 127 | } 128 | 129 | void argo_setComment(char * sw, char * text) { 130 | int i; 131 | for (i = 0; i < regs.idx; i++) { 132 | if (strcmp(sw, regs.at[i].sw) == 0) { 133 | sprintf(regs.at[i].comment, "-%s: %s", sw, text); 134 | } 135 | } 136 | } 137 | 138 | void argo_setCommentId(int id, char * text){ 139 | sprintf(regs.at[id].comment, "--%s: %s", regs.at[id].sw, text); 140 | } 141 | void argo_setExample(char * exe, char * switchs){ 142 | sprintf(example, "Example:\n %s %s",exe, switchs); 143 | } 144 | void argo_doProcess(int argc, char * argv[], int start) { 145 | int i, j; 146 | int done; 147 | int error = 0; 148 | for (i = start + 1; i < argc; i++) { 149 | 150 | /* switch format */ 151 | if (argv[i][0] != '-' || (argv[i][1] != '-')) { 152 | continue; 153 | } 154 | 155 | /* switch existence */ 156 | done = 0; 157 | for (j = 0; j < regs.idx; j++) { 158 | if (strcmp(&argv[i][2], "h") == 0) { 159 | error = 1; 160 | done = 1; 161 | break; 162 | } 163 | 164 | if (strcmp(&argv[i][2], regs.at[j].sw) == 0) { 165 | done = 1; 166 | if (regs.at[j].type == ARGON_INT) { 167 | if (regs.at[j].need_value) { 168 | if (i == argc - 1) { 169 | error = 4; 170 | break; 171 | } else { 172 | *((int *) regs.at[j].var) = atoi(argv[i + 1]); 173 | regs.at[j].set = 1; 174 | i++; 175 | } 176 | } else { 177 | *((int *) regs.at[j].var) = 1; 178 | regs.at[j].set = 1; 179 | } 180 | } else if (regs.at[j].type == ARGON_DOUBLE) { 181 | if (i == argc - 1) { 182 | error = 4; 183 | break; 184 | } else { 185 | (*((double *) regs.at[j].var))=atof(argv[i + 1]); 186 | regs.at[j].set = 1; 187 | i++; 188 | } 189 | } else if (regs.at[j].type == ARGON_STRING) { 190 | if (i == argc - 1) { 191 | error = 4; 192 | break; 193 | } else { 194 | strcpy(((char *) regs.at[j].var), argv[i + 1]); 195 | regs.at[j].set = 1; 196 | i++; 197 | } 198 | } 199 | } 200 | } 201 | 202 | if (!done) { 203 | WMP_ERROR(stderr, "*** Inexistent switch %s specified\n",argv[i]); 204 | error = 3; 205 | break; 206 | } 207 | 208 | } 209 | for (i = 0; i< regs.idx ; i++){ 210 | if (regs.at[i].mandatory && !regs.at[i].set){ 211 | WMP_ERROR(stderr, "*** Parameter %s is mandatory\n", regs.at[i].sw); 212 | error = 8; 213 | } 214 | } 215 | 216 | if (error == 4) { 217 | WMP_ERROR(stderr, "*** Incorrect number of parameters\n"); 218 | } 219 | if (error != 0) { 220 | WMP_ERROR(stderr, "List of switches:\n"); 221 | for (i = 0; i < regs.idx; i++) { 222 | WMP_ERROR(stderr, "%s\n", regs.at[i].comment); 223 | } 224 | if (strlen(example) > 0){ 225 | fprintf(stderr,"%s\n",example); 226 | } 227 | exit(1); 228 | } 229 | } 230 | #endif /* ARGON_H_ */ 231 | -------------------------------------------------------------------------------- /src/libwrapper/include/TopicManager.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #ifndef _TOPIC_MANAGER_ 31 | #define _TOPIC_MANAGER_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include "../interface/interface.h" 39 | #include "Manager.h" 40 | #include 41 | #include 42 | #include "ShapeShifterMessage.h" 43 | #include 44 | 45 | template class TopicManager: public Manager { 46 | protected: 47 | ros::Subscriber sub; 48 | ros::Publisher pub; 49 | unsigned int dest; 50 | bool im_source, im_dest; 51 | bool started; 52 | unsigned char src; 53 | std::string dest_str; 54 | public: 55 | 56 | TopicManager(ros::NodeHandle & m, unsigned int port, std::string topic, unsigned char src, std::string dest, unsigned char priority, unsigned short deadline, unsigned short period){ 57 | init(m, port, topic,src,dest,priority,deadline, period); 58 | } 59 | TopicManager(){} 60 | 61 | virtual void init(ros::NodeHandle & m, unsigned int port, std::string topic, unsigned char src, std::string dest, unsigned char priority, unsigned short deadline, unsigned short period){ 62 | this->n = m; 63 | this->port = port; 64 | this->priority = priority; 65 | this->deadline = deadline; 66 | this->period = period; 67 | this->topic = topic; 68 | this->server = src; 69 | this->type = TOPIC; 70 | this->src = src; 71 | this->dest_str = dest; 72 | im_source = src == wrapper_get_node_id(); 73 | im_dest = parse_ids(dest_str, dests); 74 | this->dest = compute_broadcast_destination(dests); 75 | started = true; 76 | } 77 | 78 | virtual std::string get_in_topic(){ 79 | std::ostringstream s; 80 | //s << n.getNamespace() << "/tx/" << topic; 81 | s << topic; 82 | return s.str(); 83 | } 84 | 85 | virtual std::string get_out_topic(){ 86 | std::ostringstream s; 87 | s << n.getNamespace() << "/rx/R" << (int) server << "/" << topic; 88 | return s.str(); 89 | } 90 | 91 | void start(){ 92 | started = true; 93 | } 94 | 95 | void reconnect(){ 96 | started = true; 97 | sub.shutdown(); 98 | sub = n.subscribe(get_in_topic(), 50, &TopicManager::callback, this); 99 | 100 | } 101 | 102 | void stop(){ 103 | started = false; 104 | } 105 | 106 | virtual void run() { 107 | wrapper_flow_add(port, period, priority, deadline); 108 | if (im_source) { 109 | //sub = n.subscribe(get_in_topic(), 1000, &TopicManager::callback, this,ros::TransportHints().tcpNoDelay() ); 110 | sub = n.subscribe(get_in_topic(), 50, &TopicManager::callback, this);//,ros::TransportHints().tcpNoDelay() ); 111 | } 112 | if (im_dest){ 113 | pub = n.advertise (get_out_topic(),1000); 114 | boost::thread(boost::bind(&TopicManager::listen, this)); 115 | } 116 | } 117 | 118 | virtual unsigned char getPriority(const boost::shared_ptr & message){ 119 | return priority; 120 | } 121 | virtual unsigned char getPriority(T & message){ 122 | return priority; 123 | } 124 | virtual unsigned int getDest(T & message){ 125 | return dest; 126 | } 127 | virtual unsigned int getDest(const boost::shared_ptr & message){ 128 | return dest; 129 | } 130 | 131 | std::vector ticks; 132 | int count = 0; 133 | 134 | bool is_time_to_push(){ 135 | 136 | ticks.push_back(TimeSpec::timestamp_ms()); 137 | if (ticks.size() > 5){ 138 | ticks.erase(ticks.begin()); 139 | } 140 | 141 | unsigned int sum = 0; 142 | for (int i = 0; i< ticks.size()-1; i++){ 143 | sum += (ticks[i+1] - ticks[i]); 144 | } 145 | 146 | int period = ticks.size() > 1 ? sum/(ticks.size()-1): 0; 147 | if (period == 0 || this->period == 0){ 148 | return true; 149 | } 150 | 151 | int rate = int(nearbyint(double(this->period)/double(period))); 152 | int real_period = rate*period; 153 | double error = fabs(1.0-(double(real_period)/double(this->period))); 154 | if (error > 0.05){ 155 | // ROS_WARN_ONCE("Topic '%s' real period is %dms", topic.c_str(), real_period); 156 | } 157 | if (++count > rate-1){ 158 | count = 0; 159 | return true; 160 | } 161 | return false; 162 | } 163 | 164 | virtual void callback(const boost::shared_ptr & message) { 165 | if (is_time_to_push()){ 166 | push(message); 167 | } 168 | } 169 | 170 | virtual void push(const boost::shared_ptr & message){ 171 | if (started){ 172 | unsigned int serial_size = ros::serialization::serializationLength(*message); 173 | char * buffer = new char[serial_size + sizeof(WrapperHeader)]; 174 | WrapperHeader * wh = (WrapperHeader *) buffer; 175 | 176 | ros::serialization::OStream stream((uint8_t *) &wh->data[0], serial_size); 177 | ros::serialization::serialize(stream, *message); 178 | 179 | int priority = getPriority(message); 180 | unsigned int dest = getDest(message); 181 | 182 | bz_push(buffer, serial_size + sizeof(WrapperHeader), priority, port, wrapper_get_node_id(), dest, deadline); 183 | delete buffer; 184 | } 185 | } 186 | 187 | virtual bool pop(T & pm){ 188 | unsigned int size; 189 | unsigned char src, priority; 190 | char * data; 191 | void * e = bz_pop(port,0, data, size, src, priority); 192 | 193 | if (e == NULL){ 194 | ROS_WARN_ONCE("Fuction pop must NOT return null for the data to be used"); 195 | return false; 196 | } 197 | 198 | WrapperHeader * wh = (WrapperHeader *) data; 199 | ros::serialization::IStream stream((uint8_t*) &wh->data[0], size - sizeof(WrapperHeader)); 200 | ros::serialization::deserialize(stream, pm); 201 | 202 | bz_pop_done(e); 203 | return true; 204 | } 205 | virtual void listen() { 206 | T pm; 207 | while (ros::ok()) { 208 | if (pop(pm)){ 209 | pub.publish(pm); 210 | } 211 | } 212 | } 213 | 214 | }; 215 | #endif 216 | -------------------------------------------------------------------------------- /src/libwrapper/include/Manager.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #ifndef _MANAGER_H__ 31 | #define _MANAGER_H__ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #define SET_BIT(array, bit) (array |= (1 << bit)) 38 | 39 | #include 40 | #include "../interface/interface.h" 41 | #include "../include/TimeSpec.h" 42 | #include "ShapeShifterMessage.h" 43 | #include "bzlib.h" 44 | 45 | class Manager { 46 | public: 47 | enum obj_type_t {TOPIC, SERVICE, SS_LISTENER}; 48 | 49 | protected: 50 | unsigned int port; 51 | unsigned char priority; 52 | unsigned short deadline; 53 | unsigned short period; 54 | ros::NodeHandle n; 55 | std::string topic; 56 | unsigned char server; 57 | obj_type_t type; 58 | int max_message_length = 0; 59 | int byte_total = 0; 60 | bool compress; 61 | std::vector sources, dests; 62 | 63 | public: 64 | int get_max_message_length(){ 65 | return max_message_length; 66 | } 67 | 68 | int get_total_bytes(){ 69 | return byte_total; 70 | } 71 | 72 | virtual void reconnect(){} 73 | virtual void start(){} 74 | virtual void stop(){} 75 | virtual void run(){} 76 | 77 | 78 | unsigned char get_server(){ 79 | return server; 80 | } 81 | 82 | obj_type_t get_type(){ 83 | return type; 84 | } 85 | 86 | std::string get_topic(){ 87 | return topic; 88 | } 89 | virtual int get_port(){ 90 | return port; 91 | } 92 | virtual int get_period(){ 93 | return period; 94 | } 95 | 96 | virtual int set_period(int period){ 97 | return this->period = wrapper_set_period(port, period); 98 | } 99 | 100 | virtual int get_priority(){ 101 | return priority; 102 | } 103 | 104 | virtual int set_priority(int priority){ 105 | return this->priority = wrapper_set_priority(port, priority); 106 | } 107 | 108 | 109 | std::vector & get_destinations(){ 110 | return dests; 111 | } 112 | 113 | bool parse_ids(std::string source, std::vector & ids){ 114 | bool im_in = false; 115 | std::vector strs; 116 | boost::split(strs, source, boost::is_any_of(" ,n/")); 117 | for (unsigned i = 0; i < strs.size();i++){ 118 | int id = atoi(strs.at(i).c_str()); 119 | ids.push_back(id); 120 | if (id == wrapper_get_node_id()){ 121 | im_in = true; 122 | } 123 | } 124 | return im_in; 125 | } 126 | 127 | Manager(){ 128 | compress = 0; 129 | } 130 | unsigned int compute_broadcast_destination(std::vector & ids){ 131 | unsigned int dest = 0; 132 | for (unsigned i = 0; i < ids.size();i++){ 133 | SET_BIT(dest, ids[i]); 134 | } 135 | return dest; 136 | } 137 | 138 | template bool deserialize(char * p, int size, Q & pm){ 139 | 140 | static boost::shared_array dbuff; 141 | static int current_size = 0; 142 | if (size > current_size){ 143 | dbuff.reset(new uint8_t[size]); 144 | current_size = size; 145 | } 146 | memcpy(dbuff.get(),p, size); 147 | 148 | try { 149 | ros::SerializedMessage smsg(dbuff, size); 150 | ros::serialization::deserializeMessage(smsg, pm); 151 | ros::Time* tm = ros::message_traits::timeStamp(pm); 152 | if (tm){ 153 | ROSWMP_DEBUG(stderr,"Type %s, tm", ros::message_traits::datatype(pm)); 154 | *tm = ros::Time::now(); 155 | }else{ 156 | ROSWMP_DEBUG(stderr,"Type %s, no TS", ros::message_traits::datatype(pm)); 157 | } 158 | 159 | return true; 160 | } catch (...){ 161 | ROS_ERROR("DESERIALIZE ERROR IN %s\n", topic.c_str()); 162 | return false; 163 | } 164 | } 165 | 166 | void set_compression(bool value){ 167 | compress = value; 168 | } 169 | 170 | bool get_compression(){ 171 | return compress; 172 | } 173 | 174 | int bz_push(char * data, unsigned int size, unsigned char priority, unsigned char port, unsigned char src, unsigned int dst, unsigned int deadline){ 175 | 176 | WrapperHeader * wh = (WrapperHeader *) data; 177 | wh->uncompressed_size = 0; 178 | 179 | if (compress){ 180 | unsigned int payload = size - sizeof(WrapperHeader); 181 | unsigned int cmp_size = payload; 182 | 183 | char * buff = new char [cmp_size]; 184 | int bz_result = BZ2_bzBuffToBuffCompress(buff, &cmp_size, &wh->data[0], payload, 1, 0, 30); 185 | 186 | if (bz_result == BZ_OUTBUFF_FULL){ 187 | ROS_INFO_ONCE("Port %d -> negative compression, sending uncompressed", port); 188 | }else if (bz_result != 0){ 189 | ROS_WARN_THROTTLE(1000,"Port %d -> compression error (%d), sending uncompressed", port, bz_result); 190 | }else{ 191 | wh->uncompressed_size = payload; 192 | memcpy(&wh->data[0], buff, cmp_size); 193 | size = cmp_size + sizeof(WrapperHeader); 194 | } 195 | delete [] buff; 196 | } 197 | return wrapper_push(data, size, priority, port, src,dst,deadline); 198 | } 199 | 200 | void * bz_pop(int port, int timeout, char* & data, unsigned int & size, unsigned char & src, unsigned char & priority){ 201 | 202 | 203 | std::pair * pnt = new std::pair(); 204 | pnt->first = wrapper_pop(port, timeout, data, size, src, priority); 205 | pnt->second = 0; 206 | 207 | WrapperHeader * wh1 = (WrapperHeader *) data; 208 | 209 | if (wh1->uncompressed_size >0){ 210 | WrapperHeader * wh2 = (WrapperHeader *) new char[wh1->uncompressed_size + sizeof(WrapperHeader)]; 211 | int bz_result = BZ2_bzBuffToBuffDecompress(&wh2->data[0], &wh1->uncompressed_size, &wh1->data[0], size - sizeof(WrapperHeader), 0, 0); 212 | if (bz_result != 0){ 213 | ROS_ERROR_THROTTLE(1000,"Port %d -> decompression error (%d)", port, bz_result); 214 | pnt->first = 0; 215 | }else{ 216 | data = pnt->second = (char *) wh2; 217 | size = wh1->uncompressed_size; 218 | memcpy(wh2, wh1, sizeof(WrapperHeader)); 219 | } 220 | } 221 | return pnt; 222 | } 223 | 224 | int bz_pop_done(void * data){ 225 | std::pair * pnt = (std::pair *) data; 226 | if (pnt->second){ 227 | delete pnt->second; 228 | } 229 | return wrapper_pop_done(pnt->first); 230 | } 231 | 232 | }; 233 | #endif 234 | -------------------------------------------------------------------------------- /src/utils/Argo_old.h: -------------------------------------------------------------------------------- 1 | #ifndef ARGO 2 | #define ARGO 3 | #include 4 | #include 5 | #include 6 | 7 | class Argo{ 8 | 9 | class Param{ 10 | public: 11 | enum types_t {INT, FLOAT, STRING, BOOL}; 12 | protected: 13 | types_t type; 14 | bool has_parameter, mandatory; 15 | std::string name, description; 16 | std::string hr_type; 17 | public: 18 | void init(std::string name, std::string description, bool has_parameter, bool mandatory){ 19 | this->name = name; 20 | this->description = description; 21 | this->has_parameter = has_parameter; 22 | this->mandatory = mandatory; 23 | } 24 | std::string getName(){ 25 | return name; 26 | } 27 | std::string getDescription(){ 28 | return description; 29 | } 30 | bool isMandatory(){ 31 | return mandatory; 32 | } 33 | bool hasParameter(){ 34 | return has_parameter; 35 | } 36 | virtual std::string getHRValue(){ 37 | } 38 | virtual std::string getHRType(){ 39 | } 40 | types_t getType(){ 41 | return type; 42 | } 43 | virtual void setValue(std::string ){} 44 | }; 45 | 46 | template class GenParam :public Param{ 47 | T & value; 48 | T dflt; 49 | void setHRType(){ 50 | const std::type_info & t = typeid(T); 51 | if (t == typeid(int) 52 | || t == typeid(unsigned int) 53 | || t == typeid(char) 54 | || t == typeid(unsigned char) 55 | || t == typeid(short) 56 | || t == typeid(unsigned short) 57 | || t == typeid(long int) 58 | || t == typeid(unsigned long long int) 59 | || t == typeid(long long int) 60 | || t == typeid(unsigned long int)){ 61 | hr_type = "integer"; 62 | type = INT; 63 | }else if (t == typeid(double) || t == typeid(float)){ 64 | hr_type = "float "; 65 | type = FLOAT; 66 | }else if (t == typeid(bool)){ 67 | hr_type = "boolean"; 68 | type = BOOL; 69 | }else if (t == typeid(std::string)){ 70 | hr_type = "string "; 71 | type = STRING; 72 | } 73 | } 74 | public: 75 | GenParam(T & thevalue, T dflt, std::string name, std::string description, bool has_parameter, bool mandatory): value(thevalue){ 76 | this->dflt = dflt; 77 | this->value = dflt; 78 | init(name, description, has_parameter, mandatory); 79 | setHRType(); 80 | } 81 | virtual void setValue(std::string s){ 82 | std::istringstream iss(s); 83 | iss >> value; 84 | } 85 | virtual std::string getHRValue(){ 86 | std::ostringstream oss; 87 | oss << name <<" = " << value ; 88 | return oss.str(); 89 | } 90 | std::string demangle(const char* mangled) 91 | { 92 | int status; 93 | std::unique_ptr result( 94 | abi::__cxa_demangle(mangled, 0, 0, &status), std::free); 95 | return result.get() ? std::string(result.get()) : "error occurred"; 96 | } 97 | std::string getHRType(){ 98 | // const char * p= typeid(T).name(); 99 | // return demangle(p); 100 | return hr_type; 101 | } 102 | }; 103 | std::vector vec; 104 | int helper, maxlen; 105 | std::string example; 106 | public: 107 | 108 | void showList(){ 109 | std::cerr << "List of all options (* are mandatory):" << std::endl; 110 | for (int j = 0; j< vec.size(); j++){ 111 | Param * p = vec[j]; 112 | std::cerr << " " << std::left<< std::setw(maxlen+1) << p->getName(); 113 | if (p->hasParameter()){ 114 | std::cerr << " " << p->getHRType(); 115 | }else{ 116 | std::cerr << " "; 117 | } 118 | if (p->isMandatory()){ 119 | std::cerr << " *: "; 120 | }else{ 121 | std::cerr << " : "; 122 | } 123 | std::cerr << p->getDescription() < void add(T & value, T dflt, std::string name, std::string description, bool has_parameter, bool mandatory){ 132 | GenParam *p = new GenParam(value, dflt, "-" + name, description, has_parameter, mandatory); 133 | maxlen = name.size()>maxlen?name.size():maxlen; 134 | vec.push_back(p); 135 | } 136 | void addSwitch(bool & value, bool dflt, std::string name, std::string description){ 137 | add(value, dflt,name, description, false, false); 138 | } 139 | void addStringMandatory(std::string & value, std::string name, std::string description){ 140 | add(value, "", name, description, true, true); 141 | } 142 | void addIntMandatory(int & value, std::string name, std::string description){ 143 | add(value, 0, name, description, true, true); 144 | } 145 | void addInt(int & value, int dflt, std::string name, std::string description){ 146 | add(value, dflt, name, description, true, false); 147 | } 148 | void addDouble(double & value, double dflt, std::string name, std::string description){ 149 | add(value, dflt, name, description, true, false); 150 | } 151 | void addString(std::string & value,std::string dflt, std::string name, std::string description){ 152 | add(value, dflt, name, description, true, false); 153 | } 154 | void process(int argc, char * argv[]){ 155 | 156 | for (int i = 1; i< argc; i++){ 157 | if(strcmp(argv[i],"-h") == 0){ 158 | showList(); 159 | exit(0); 160 | } 161 | } 162 | 163 | for (int j = 1; j< argc; j++){ 164 | if (argv[j][0]=='-'){ 165 | bool found = 0; 166 | for (int i = 0; i< vec.size(); i++){ 167 | Param * p = vec[i]; 168 | if(p->getName().compare(argv[j]) == 0){ 169 | found = 1; 170 | } 171 | } 172 | if (! found){ 173 | std::cerr << "*** Non existent switch " << argv[j] << std::endl; 174 | if (atoi(argv[j])!=0){ 175 | std::cerr << "*** To insert negative numbers use '+' prefix, e.g. +" << argv[j] << std::endl; 176 | } 177 | showList(); 178 | exit(0); 179 | } 180 | }else if(argv[j][0]=='+'){ 181 | argv[j][0] = ' '; 182 | } 183 | } 184 | 185 | for (int i = 0; i< vec.size(); i++){ 186 | bool found = 0; 187 | Param * p = vec[i]; 188 | for (int j = 1; j< argc; j++){ 189 | if(p->getName().compare(argv[j]) == 0){ 190 | found = 1; 191 | if (p->hasParameter()){ 192 | if (j+1 >= argc){ 193 | std::cerr << "*** It seems you forgot the parameter for " << p->getName() << std::endl; 194 | showList(); 195 | exit(0); 196 | }else{ 197 | for (int k = 1; k< vec.size(); k++){ 198 | Param * q = vec[k]; 199 | if(q->getName().compare(argv[j+1]) == 0){ 200 | std::cerr << "*** It seems you forgot the parameter for " << p->getName() << std::endl; 201 | showList(); 202 | exit(0); 203 | } 204 | } 205 | } 206 | p->setValue(std::string(argv[j+1])); 207 | }else{ 208 | p->setValue("1"); 209 | } 210 | } 211 | } 212 | if (p->isMandatory() && found == 0){ 213 | std::cerr << "*** Parameter for " << p->getName() << " is mandatory" << std::endl; 214 | showList(); 215 | exit(0); 216 | } 217 | } 218 | for (int i = 1; i< argc; i++){ 219 | if(strcmp(argv[i],"-v") == 0){ 220 | for (int j = 2; j< vec.size(); j++){ 221 | std::cerr << vec[j]->getHRValue() << std::endl; 222 | } 223 | } 224 | } 225 | } 226 | 227 | void addExample(std::string example){ 228 | this->example = example; 229 | } 230 | Argo(){ 231 | maxlen = 0; 232 | add(helper,0,"h", "Show this screen", false, false); 233 | add(helper,0,"v", "Show values assigned to parameters", false, false); 234 | } 235 | }; 236 | #endif 237 | -------------------------------------------------------------------------------- /src/libwrapper/include/ShapeShifterManager.h: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #ifndef _SHAPESHITFER_MANAGER_ 31 | #define _SHAPESHITFER_MANAGER_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include "../interface/interface.h" 39 | #include "Manager.h" 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include "ShapeShifterMessage.h" 45 | 46 | class ShapeShifterManager: public Manager { 47 | protected: 48 | ros::Subscriber sub; 49 | ros::Publisher pub; 50 | std::vector sources, dests; 51 | unsigned int dest; 52 | bool im_source, im_dest; 53 | bool started; 54 | unsigned char src; 55 | std::string dest_str; 56 | public: 57 | 58 | ShapeShifterManager(ros::NodeHandle & m, unsigned int port, std::string topic, unsigned char src, std::string dest, unsigned char priority, unsigned short deadline, unsigned short period){ 59 | init(m, port, topic,src,dest,priority,deadline, period); 60 | } 61 | ShapeShifterManager(){} 62 | 63 | virtual void init(ros::NodeHandle & m, unsigned int port, std::string topic, unsigned char src, std::string dest, unsigned char priority, unsigned short deadline, unsigned short period){ 64 | this->n = m; 65 | this->port = port; 66 | this->priority = priority; 67 | this->deadline = deadline; 68 | this->period = period; 69 | this->topic = topic; 70 | this->server = src; 71 | this->type = TOPIC; 72 | this->src = src; 73 | this->dest_str = dest; 74 | started = true; 75 | } 76 | 77 | 78 | virtual std::string get_in_topic(){ 79 | std::ostringstream s; 80 | s << n.getNamespace() << "/tx/" << topic; 81 | return s.str(); 82 | } 83 | 84 | virtual std::string get_out_topic(){ 85 | std::ostringstream s; 86 | s << n.getNamespace() << "/rx/R" << (int) server << "/" << topic; 87 | return s.str(); 88 | } 89 | 90 | void start(){ 91 | started = true; 92 | } 93 | 94 | 95 | void stop(){ 96 | started = false; 97 | } 98 | 99 | virtual void run() { 100 | im_source = src == wrapper_get_node_id(); 101 | im_dest = parse_ids(dest_str, dests); 102 | this->dest = compute_broadcast_destination(dests); 103 | wrapper_flow_add(port, period, priority, deadline); 104 | 105 | if (im_source) { 106 | sub = n.subscribe(get_in_topic(), 1000, &ShapeShifterManager::callback, this); 107 | } 108 | if (im_dest){ 109 | // pub = n.advertise (get_out_topic(),1000); 110 | boost::thread(boost::bind(&ShapeShifterManager::listen, this)); 111 | } 112 | } 113 | 114 | // virtual unsigned char getPriority(const boost::shared_ptr & message){ 115 | // return priority; 116 | // } 117 | // virtual unsigned char getPriority(T & message){ 118 | // return priority; 119 | // } 120 | // virtual unsigned int getDest(T & message){ 121 | // return dest; 122 | // } 123 | // virtual unsigned int getDest(const boost::shared_ptr & message){ 124 | // return dest; 125 | // } 126 | 127 | std::vector ticks; 128 | int count = 0; 129 | 130 | bool is_time_to_push(){ 131 | 132 | ticks.push_back(TimeSpec::timestamp_ms()); 133 | if (ticks.size() > 5){ 134 | ticks.erase(ticks.begin()); 135 | } 136 | 137 | unsigned int sum = 0; 138 | for (int i = 0; i< ticks.size()-1; i++){ 139 | sum += (ticks[i+1] - ticks[i]); 140 | } 141 | 142 | int period = ticks.size() > 1 ? sum/(ticks.size()-1): 0; 143 | if (period == 0 || this->period == 0){ 144 | return true; 145 | } 146 | 147 | int rate = int(nearbyint(double(this->period)/double(period))); 148 | int real_period = rate*period; 149 | double error = fabs(1.0-(double(real_period)/double(this->period))); 150 | if (error > 0.05){ 151 | ROS_WARN_ONCE("Topic '%s' real period is %dms", topic.c_str(), real_period); 152 | } 153 | if (++count > rate-1){ 154 | count = 0; 155 | return true; 156 | } 157 | return false; 158 | } 159 | 160 | virtual void callback(const topic_tools::ShapeShifter::ConstPtr& message) { 161 | if (is_time_to_push()){ 162 | push(message); 163 | } 164 | } 165 | 166 | virtual void push(const topic_tools::ShapeShifter::ConstPtr& msg){ 167 | if (started){ 168 | std::ostringstream oss; 169 | 170 | int extra_length = + msg->getDataType().size() + msg->getMD5Sum().size() + msg->getMessageDefinition().size() + 2; 171 | std::vector buffer(msg->size() + extra_length + sizeof(ShapeShifterMessage)); 172 | ShapeShifterMessage * ssm = (ShapeShifterMessage *) &buffer[0]; 173 | ssm->size = msg->size(); 174 | ssm->datatype_size = msg->getDataType().size(); 175 | ssm->msg_def_size = msg->getMessageDefinition().size(); 176 | 177 | oss << msg->getMD5Sum() << msg->getDataType() << msg->getMessageDefinition(); 178 | 179 | unsigned char * pointer = &buffer[0] + sizeof(ShapeShifterMessage); 180 | ros::serialization::OStream ostream(pointer, msg->size()); 181 | msg->write(ostream); 182 | 183 | pointer += msg->size(); 184 | memcpy(pointer, oss.str().c_str(), extra_length); 185 | fprintf(stderr,"test: %s\n",pointer); 186 | 187 | //https://github.com/juancamilog/rosserial-mrl/blob/master/rosserial_server/src/topic_handlers.h 188 | 189 | wrapper_push((char *) &buffer[0], buffer.size(), priority, port, wrapper_get_node_id(), dest, deadline); 190 | } 191 | } 192 | 193 | // virtual void push(T & message){ 194 | // if (started){ 195 | // ros::SerializedMessage buff = ros::serialization::serializeMessage(message); 196 | // ROSWMP_DEBUG(stderr,"Serializing size %d\n",buff.num_bytes); 197 | // int priority = getPriority(message); 198 | // unsigned int dest = getDest(message); 199 | // wrapper_push((char *) buff.message_start, buff.num_bytes, priority, port, wrapper_get_node_id(), dest, deadline); 200 | // } 201 | // } 202 | 203 | virtual bool pop(boost::shared_ptr & shapeShifter){ 204 | unsigned int size; 205 | unsigned char src, priority; 206 | char * data; 207 | void * e = wrapper_pop(port,0, data, size, src,priority); 208 | 209 | ShapeShifterMessage * ssm = (ShapeShifterMessage *) data; 210 | unsigned char * pointer = (unsigned char *) data + sizeof(ShapeShifterMessage); 211 | ros::serialization::IStream istream(pointer, ssm->size); 212 | pointer += ssm->size; 213 | 214 | std::string metadata((const char * )pointer); 215 | 216 | shapeShifter->morph(metadata.substr(0,32), metadata.substr(32, ssm->datatype_size), metadata.substr(32 + ssm->datatype_size), "false"); 217 | shapeShifter->read(istream); 218 | 219 | std::cerr << metadata.substr(0,32) << " --- " << metadata.substr(32, ssm->datatype_size) << "---" << metadata.substr(32 + ssm->datatype_size) << std::endl; 220 | 221 | if (e == NULL){ 222 | ROS_WARN_ONCE("Fuction pop must NOT return null for the data to be used"); 223 | return false; 224 | } 225 | 226 | wrapper_pop_done(e); 227 | return true; 228 | } 229 | virtual void listen() { 230 | boost::shared_ptr shapeShifter(new topic_tools::ShapeShifter); 231 | while (ros::ok()) { 232 | if (pop(shapeShifter)){ 233 | static ros::Publisher pub = shapeShifter->advertise(n,get_out_topic(),1); 234 | pub.publish(shapeShifter); 235 | } 236 | } 237 | } 238 | 239 | }; 240 | #endif 241 | -------------------------------------------------------------------------------- /src/libwrapper/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* ---------------------------------------------------------------------- 3 | * Copyright (C) 2000-2016, Universidad de Zaragoza, SPAIN 4 | * 5 | * Contact Addresses: Danilo Tardioli dantard@unizar.es 6 | * 7 | * This is free software; you can redistribute it and/or modify it 8 | * under the terms of the GNU General Public License as published by the 9 | * Free Software Foundation; either version 2, or (at your option) any 10 | * later version. 11 | * 12 | * This software is distributed in the hope that it will be useful, but 13 | * WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15 | * General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * distributed with RT-WMP; see file COPYING. If not, write to the 19 | * Free Software Foundation, 59 Temple Place - Suite 330, Boston, MA 20 | * 02111-1307, USA. 21 | * 22 | * As a special exception, if you link this unit with other files to 23 | * produce an executable, this unit does not by itself cause the 24 | * resulting executable to be covered by the GNU General Public License. 25 | * This exception does not however invalidate any other reasons why the 26 | * executable file might be covered by the GNU Public License. 27 | * 28 | *----------------------------------------------------------------------*/ 29 | 30 | #include 31 | 32 | #include "include/ServiceManager.h" 33 | #include "include/TFTopicManager.h" 34 | #include "include/QOSTopicManager.h" 35 | #include "include/TopicManager.h" 36 | #include "include/ShapeShifterManager.h" 37 | #include "include/ShapeShifterSender.h" 38 | #include "include/ShapeShifterListener.h" 39 | 40 | #include "ros/ros.h" 41 | #include "libwrapper/Manage.h" 42 | #include "include/argon.h" 43 | #include "config/data_types.h" 44 | #include 45 | 46 | 47 | #define VTOPIC(type, topic, source, dest, priority, period, time_to_live) map[topic] = new TopicManager(n, port, topic,source,std::vector(dest),priority,time_to_live, period); port++; 48 | #define TOPIC(type, topic, source, dest, priority, period, time_to_live) map[topic] = new TopicManager(n, port, topic,source,dest,priority,time_to_live, period); port++; 49 | #define QOSTOPIC(type, topic, source, dest, priority, period, time_to_live, queue_length) map[topic] = new QOSTopicManager(n, port, topic,source,dest,priority,time_to_live, period, queue_length); port++; 50 | #define TFTOPIC(topic, source, dest, priority, period, time_to_live) map[topic] = new TFTopicManager(n, port, topic, source, dest, priority, time_to_live, period); port++; 51 | #define SERVICE(type, topic, source, priority, time_to_live) map[topic] = new ServiceManager(n, port, topic, source, priority, time_to_live); port+=2; 52 | #define TOPIC_STOP(topic); map[topic]->stop(); 53 | #define SSTOPIC(type, topic, source, dest, priority, period, time_to_live) map[topic] = new ShapeShifterManager(n, port, topic,source,dest,priority,time_to_live, period); port++; 54 | #define TF_ADD_WHITELIST(topic, elem); ((TFTopicManager *) map[topic])->add_whitelist(elem); 55 | #define TF_ADD_BLACKLIST(topic, elem); ((TFTopicManager *) map[topic])->add_blacklist(elem); 56 | #define TF_OUT_TOPIC(topic, elem); ((TFTopicManager *) map[topic])->set_out_topic(elem); 57 | 58 | int node_id, num_of_nodes; 59 | std::map::iterator it; 60 | std::map map; 61 | std::vector sms; 62 | 63 | bool callback(typename libwrapper::Manage::Request &req, typename libwrapper::Manage::Response &resp) { 64 | 65 | std::transform(req.command.begin(), req.command.end(),req.command.begin(), ::toupper); 66 | 67 | if (req.command.at(0) == '_'){ 68 | return wrapper_call_service(req.command, req.param1, req.param2, resp.info, resp.result); 69 | }else if(req.command.compare("LIST") == 0){ 70 | resp.info.clear(); 71 | resp.info.append("LIST, GET_NODE_ID, ECHO, START, STOP, RECONNECT, SET_PERIOD, SET_PRIORITY, GET_PRIORITY, GET_PERIOD"); 72 | resp.result = 0; 73 | return true; 74 | }else if(req.command.compare("GET_NODE_ID") == 0){ 75 | resp.info.append("OK"); 76 | resp.result = wrapper_get_node_id(); 77 | return true; 78 | }else if(req.command.compare("ECHO") == 0){ 79 | resp.info.append(req.param1); 80 | resp.result = 0; 81 | return true; 82 | } 83 | 84 | auto it = map.find(req.param1); 85 | if (it == map.end()){ 86 | resp.info.append("INEXISTENT TOPIC/SERVICE or COMMAND"); 87 | resp.result = -1; 88 | return true; 89 | } 90 | 91 | if (it->second->get_server() != wrapper_get_node_id()){ 92 | resp.info.append("TOPIC/SERVICE *NOT* ON THIS NODE"); 93 | resp.result = -2; 94 | return true; 95 | } 96 | 97 | if (it->second->get_type() != Manager::TOPIC){ 98 | resp.info.append("THIS COMMAND IS ONLY AVAILABLE FOR TOPICS"); 99 | resp.result = -4; 100 | return true; 101 | } 102 | 103 | 104 | resp.info.append("OK"); 105 | resp.result = 0; 106 | 107 | if (req.command.compare("STOP") == 0){ 108 | 109 | it->second->stop(); 110 | return true; 111 | 112 | }else if(req.command.compare("START") == 0){ 113 | 114 | it->second->start(); 115 | return true; 116 | 117 | }else if(req.command.compare("SET_PERIOD") == 0){ 118 | 119 | resp.result = it->second->set_period(req.param2); 120 | 121 | return true; 122 | 123 | }else if(req.command.compare("SET_PRIORITY") == 0){ 124 | 125 | resp.result = it->second->set_priority(req.param2); 126 | return true; 127 | 128 | }else if(req.command.compare("GET_PRIORITY") == 0){ 129 | 130 | resp.result = it->second->get_priority(); 131 | return true; 132 | 133 | }else if(req.command.compare("GET_PERIOD") == 0){ 134 | 135 | resp.result = it->second->get_period(); 136 | return true; 137 | }else if(req.command.compare("GET_MAX_MESSAGE_LENGTH") == 0){ 138 | 139 | resp.result = it->second->get_max_message_length(); 140 | return true; 141 | }else if(req.command.compare("GET_TOTAL_BYTES") == 0){ 142 | 143 | resp.result = it->second->get_total_bytes(); 144 | return true; 145 | }else if(req.command.compare("RECONNECT") == 0){ 146 | it->second->reconnect(); 147 | return true; 148 | } 149 | 150 | resp.info.append("INEXISTENT COMMAND"); 151 | resp.result = -3; 152 | return true; 153 | 154 | } 155 | 156 | void create_shapeshifter_topics(ros::NodeHandle &n, int port, YAML::Node & topics){ 157 | for (int i = 0; i(); 162 | }else{ 163 | ROS_ERROR("Field 'name' is mandatory in a topic definition"); 164 | exit(1); 165 | } 166 | std::string dest; 167 | if (YAML::Node parameter = topic["dest"]){ 168 | dest = parameter.as(); 169 | }else{ 170 | ROS_ERROR("Field 'dest' is mandatory in a topic definition"); 171 | exit(1); 172 | } 173 | int priority = 0; 174 | if (YAML::Node parameter = topic["priority"]){ 175 | priority = parameter.as(); 176 | } 177 | int period = 1000; 178 | if (YAML::Node parameter = topic["period"]){ 179 | period = parameter.as(); 180 | } 181 | int source = 0; 182 | if (YAML::Node parameter = topic["source"]){ 183 | source = parameter.as(); 184 | } 185 | int deadline = 1000; 186 | if (YAML::Node parameter = topic["deadline"]){ 187 | deadline = parameter.as(); 188 | } 189 | 190 | map[name] = new ShapeShifterSender(n, port, name, dest, priority, deadline, period); 191 | 192 | if (YAML::Node parameter = topic["use_compression"]){ 193 | bool use_compression = parameter.as(); 194 | map[name]->set_compression(use_compression); 195 | } 196 | 197 | } 198 | std::cout << "Added " << topics.size() << " shapeshifter topics" << std::endl; 199 | 200 | } 201 | 202 | int main(int argc, char * argv[]){ 203 | 204 | char ns[32], name[32], path[32]; 205 | int ans, delay; 206 | argo_setCommentId(argo_addString(name, STR("node-name"), STR("")), 207 | STR("Specify the name of the node")); 208 | argo_setCommentId(argo_addString(ns, STR("namespace"), STR("")), 209 | STR("Specify the namespace")); 210 | argo_setCommentId(argo_addInt(&ans, STR("auto-namespace"), 0, 0), 211 | STR("Obtain the namespace from node_id")); 212 | argo_setCommentId(argo_addString(path, STR("force-path"), STR("")), 213 | STR("Specify the path of the messages")); 214 | argo_setCommentId(argo_addIntMandatory(&node_id, STR("node-id"), 0, 1), 215 | STR("Specify node address")); 216 | argo_setCommentId(argo_addIntMandatory(&num_of_nodes, STR("num-of-nodes"), 2, 1), 217 | STR("Specify number of nodes")); 218 | argo_setCommentId(argo_addInt(&delay, STR("start-delay"), 0, 1), 219 | STR("Delays the start of the node")); 220 | argo_setExample(argv[0],STR("--node-id 0 --num-of-nodes 3 --auto-namespace")); 221 | 222 | argo_doProcess(argc, argv, 0); 223 | 224 | std::ostringstream oss; 225 | oss << "R" << node_id; 226 | ros::init(argc, argv, oss.str()); 227 | 228 | ros::NodeHandle n(oss.str()); 229 | 230 | int port = 0; 231 | for (int i = 0; i< num_of_nodes; i++) { 232 | Manager * sm = new ServiceManager(n, port, "manage", i, 10, 21000); 233 | std::ostringstream oss; 234 | oss << "manage (R" << i << ")"; 235 | map[oss.str()] = sm; 236 | sms.push_back(sm); 237 | port+=2; 238 | } 239 | ros::ServiceServer service = n.advertiseService("manage", callback); 240 | map["ss_listener"] = new ShapeShifterListener(n, port); 241 | 242 | std::ostringstream oss1; 243 | std::string filename = wrapper_get_config_filename(); 244 | 245 | oss1 << getenv("HOME") << "/" <second->get_type()==Manager::TOPIC){ 270 | printf("%-40s %5d %3d %5d %3s ", it->first.c_str(), it->second->get_port(), it->second->get_priority(), it->second->get_period(),it->second->get_compression()?"yes":"no"); 271 | 272 | for (int i = 0; i< it->second->get_destinations().size(); i++){ 273 | printf("%s", i != 0 ? ", " : " "); 274 | printf("%d", it->second->get_destinations().at(i)); 275 | } 276 | printf("\n"); 277 | } 278 | 279 | 280 | } 281 | 282 | printf("\n%-40s PORTS PRIORITY PERIOD CMP\n", "SERVICES"); 283 | printf("----------------------------------------------\n"); 284 | for (it = map.begin(); it != map.end(); it++){ 285 | if (it->second->get_type()==Manager::SERVICE){ 286 | printf("%-40s %2d,%2d %3d %5s %3s\n", it->first.c_str(), it->second->get_port(), it->second->get_port()+1, it->second->get_priority(), "n/a",it->second->get_compression()?"yes":"no"); 287 | } 288 | } 289 | printf("\n"); 290 | wrapper_run_bg(); 291 | 292 | 293 | for (it = map.begin(); it != map.end(); it++){ 294 | map[it->first]->run(); 295 | } 296 | 297 | 298 | 299 | ///XXXXXXXXXXXXXXXXXXXXXXXX 300 | ros::AsyncSpinner spinner(4); // Use 4 threads 301 | // while (ros::ok()){ 302 | // ros::spinOnce(); 303 | // usleep(10); 304 | // } 305 | 306 | spinner.start(); 307 | ros::waitForShutdown(); 308 | } 309 | 310 | int wrapper_get_node_id(){ 311 | 312 | return node_id; 313 | } 314 | 315 | -------------------------------------------------------------------------------- /src/utils/Argo.h: -------------------------------------------------------------------------------- 1 | #ifndef ARGO 2 | #define ARGO 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class Argo{ 10 | 11 | std::vector