├── .gitignore ├── .gitmodules ├── Add_PLC_route.PNG ├── Archi.png ├── LICENSE ├── README.md ├── ros_ads_msgs ├── CMakeLists.txt ├── include │ └── ros_ads_msgs │ │ └── ADSDecode.h ├── msg │ ├── ADS.msg │ └── State.msg ├── package.xml └── src │ └── ADSDecode.cpp └── ros_ads_node ├── CMakeLists.txt ├── config └── configuration.yaml ├── launch └── ADSnode.launch ├── package.xml └── src ├── Ads_Interface.cpp ├── Ads_Interface.h ├── Ads_node.cpp └── Ads_node.h /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | CMakeLists.txt.user 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "ros_ads_node/lib/ADS"] 2 | path = ros_ads_node/lib/ADS 3 | url = https://github.com/Capacites/ADS.git 4 | -------------------------------------------------------------------------------- /Add_PLC_route.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Capacites/ros_ADS_Beckhoff/c1c225d6714b70e13878625603e768cdd95188d7/Add_PLC_route.PNG -------------------------------------------------------------------------------- /Archi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Capacites/ros_ADS_Beckhoff/c1c225d6714b70e13878625603e768cdd95188d7/Archi.png -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 CAPACITES 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS_ADS_Beckhoff 2 | 3 | ## Overview 4 | 5 | A ROS package for communication with Beckhoff's ADS devices 6 | 7 | **Keywords:** ROS, ADS, Beckhoff ADS 8 | 9 | ### License 10 | 11 | The source code is released under a [MIT License](LICENSE). 12 | 13 | ## Installation 14 | 15 | #### Dependencies 16 | 17 | - [Robot Operating System (ROS)](http://wiki.ros.org) (middleware for robotics), 18 | - [libyaml-cpp-dev](https://github.com/jbeder/yaml-cpp) 19 | 20 | 21 | #### Building 22 | 23 | cd ros_workspace/src 24 | git clone -b main --recursive https://github.com/Capacites/ros_ADS_Beckhoff.git 25 | cd .. 26 | catkin_make 27 | 28 | ## Node 29 | 30 | 31 | #### Architecture 32 | 33 | ![Archi](Archi.png "Achitecture") 34 | 35 | 36 | The package provides a C++ node: `rosrun ros_ads_node ros_ads_node` 37 | 38 | While continuously updating the state of the ADS device's variables, the node may publish two types of messages: 39 | - The variables' state (on timer or on event depending on the configuration file) 40 | - The node state (indicating wether published data should be considered valid or not, and a debug code) 41 | 42 | ## Published Topics 43 | 44 | * **`state`** ([ros_ads_msgs/State]) 45 | 46 | Publishes the node state in a custom message, the state will be published on timer and on event: 47 | 48 | std_msgs/Header header # header info 49 | bool state # Data validity 50 | uint8 error # error number 51 | 52 | * **`report_timer`** ([ros_ads_msgs/ADS]) 53 | 54 | Publishes in a custom message the IO values on timer callback for IOs configured as such in the configuration file: 55 | 56 | std_msgs/Header header # header info 57 | string[] varNames # variables name list 58 | string[] varTypes # variables type list 59 | float64[] varValues # variables value list 60 | 61 | * **`report_event`** ([ros_ads_msgs/ADS]) 62 | 63 | Publishes in a custom message the IO values on event callback for IOs configured as such in the configuration file: 64 | 65 | std_msgs/Header header # header info 66 | string[] varNames # variables name list 67 | string[] varTypes # variables type list 68 | float64[] varValues # variables value list 69 | 70 | ## Subscribed Topics 71 | 72 | * **`command`** ([ros_ads_msgs/ADS]) 73 | 74 | Subscribes to commands, write the received values on the associated outputs of the device: 75 | 76 | std_msgs/Header header # header info 77 | string[] varNames # variables name list 78 | string[] varTypes # variables type list 79 | float64[] varValues # variables value list 80 | 81 | ## Parameters 82 | 83 | - `name` 84 | - Type: string 85 | - Description: Name of the device, must be the same in the configuration file 86 | - `config_file` 87 | - Type: string 88 | - Description: File containing the configuration to be loaded 89 | 90 | ## Configuration file 91 | 92 | A configuration file example can be found in `ros_ads_node/config` as `configuration.yaml` 93 | 94 | ## Supported types 95 | 96 | ros_ads only supports the following ADS variable types : 97 | 98 | - BOOL 99 | - BYTE 100 | - USINT 101 | - SINT 102 | - WORD 103 | - UINT 104 | - DINT 105 | - LINT 106 | - REAL 107 | - LREAL 108 | - DATE 109 | 110 | ## Utilities 111 | 112 | ros_ads_msgs provides a `decode` function to use on a [ros_ads_msgs/ADS] message and returning `std::map`. 113 | `variant_t` is defined as `std::variant`. 114 | 115 | ## Setup your ADS route 116 | 117 | ![route setup help](Add_PLC_route.PNG "route setup help") 118 | 119 | ## Limitation 120 | 121 | You can connect different devices with different remote ip to same ip on PC 122 | 123 | You can't create multiple route between same PC IP adesse and same IP device. 124 | If do this, you could have connection issue/ 125 | if you still have connection issue check if an other process use this route (pyads,.... ) . 126 | -------------------------------------------------------------------------------- /ros_ads_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ros_ads_msgs) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") 5 | 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | set(CMAKE_CXX_EXTENSIONS OFF) 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | message_generation 15 | ) 16 | 17 | add_message_files( 18 | FILES 19 | State.msg 20 | ADS.msg 21 | ) 22 | 23 | generate_messages( 24 | DEPENDENCIES 25 | std_msgs 26 | ) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | LIBRARIES ${PROJECT_NAME} 31 | CATKIN_DEPENDS message_runtime std_msgs roscpp 32 | ) 33 | 34 | include_directories(include PUBLIC ${catkin_INCLUDE_DIRS}) 35 | 36 | add_library(${PROJECT_NAME} SHARED src/ADSDecode.cpp) 37 | 38 | add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) 39 | 40 | target_link_libraries(${PROJECT_NAME} ${catkin_INCLUDE_DIRS} ${catkin_LIBRARIES}) 41 | 42 | set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) 43 | set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) 44 | 45 | install(DIRECTORY include/${PROJECT_NAME}/ 46 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 47 | ) 48 | -------------------------------------------------------------------------------- /ros_ads_msgs/include/ros_ads_msgs/ADSDecode.h: -------------------------------------------------------------------------------- 1 | #ifndef HEADER_H_ADS_MSGS 2 | #define HEADER_H_ADS_MSGS 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace ros_ads_msgs { 12 | 13 | using variant_t = std::variant; 14 | 15 | enum{ 16 | BOOL = 0, 17 | UINT8_T = 1, 18 | INT8_T = 2, 19 | UINT16_T = 3, 20 | INT16_T = 4, 21 | UINT32_T = 5, 22 | INT32_T = 6, 23 | INT64_T = 7, 24 | FLOAT = 8, 25 | DOUBLE = 9, 26 | DATE = 10, 27 | }; 28 | 29 | std::map decode(const ros_ads_msgs::ADS::ConstPtr &msg); 30 | 31 | } 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /ros_ads_msgs/msg/ADS.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header # header info 2 | string[] varNames 3 | string[] varTypes 4 | float64[] varValues 5 | -------------------------------------------------------------------------------- /ros_ads_msgs/msg/State.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header # header info 2 | bool state # Data validity 3 | uint8 error # error number 4 | -------------------------------------------------------------------------------- /ros_ads_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ros_ads_msgs 4 | 1.0.0 5 | Basic messages used to communicate with a ros_ads_node node and report it's state 6 | 7 | Alexis Fetet 8 | elvin 9 | Julien Viaud 10 | 11 | BSD 12 | 13 | 18 | 19 | catkin 20 | 21 | message_generation 22 | message_runtime 23 | std_msgs 24 | roscpp 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /ros_ads_msgs/src/ADSDecode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace ros_ads_msgs; 4 | 5 | std::map ros_ads_msgs::decode(const ros_ads_msgs::ADS::ConstPtr &msg) 6 | { 7 | std::map> result; 8 | std::string name; 9 | std::string type; 10 | double value; 11 | bool cresult; 12 | std::variant converted_value; 13 | 14 | int minSize = std::min(std::min(msg->varNames.size(), msg->varTypes.size()), msg->varValues.size()); 15 | 16 | for(int index = 0; index < minSize; index ++) 17 | { 18 | cresult = false; 19 | name = msg->varNames[index]; 20 | type = msg->varTypes[index]; 21 | value = msg->varValues[index]; 22 | do 23 | { 24 | if(type == "BOOL") 25 | { 26 | converted_value = static_cast(value); 27 | cresult = true; 28 | } 29 | if(type == "BYTE" || type == "USINT") 30 | { 31 | converted_value = static_cast(value); 32 | cresult = true; 33 | } 34 | if(type == "SINT") 35 | { 36 | converted_value = static_cast(value); 37 | cresult = true; 38 | } 39 | if(type == "WORD" || type == "UINT") 40 | { 41 | converted_value = static_cast(value); 42 | cresult = true; 43 | } 44 | if(type == "INT") 45 | { 46 | converted_value = static_cast(value); 47 | cresult = true; 48 | } 49 | if(type == "DWORD" || type == "UDINT") 50 | { 51 | converted_value = static_cast(value); 52 | cresult = true; 53 | } 54 | if(type == "DINT") 55 | { 56 | converted_value = static_cast(value); 57 | cresult = true; 58 | break; 59 | } 60 | if(type == "LINT") 61 | { 62 | converted_value = static_cast(value); 63 | cresult = true; 64 | break; 65 | } 66 | if(type == "REAL") 67 | { 68 | converted_value = static_cast(value); 69 | cresult = true; 70 | break; 71 | } 72 | if(type == "LREAL") 73 | { 74 | converted_value = static_cast(value); 75 | cresult = true; 76 | break; 77 | } 78 | if(type == "DATE") 79 | { 80 | auto temp = static_cast(value); 81 | ros::Time currentDate(temp); 82 | time_t tDate(currentDate.toSec()); 83 | tm tmDate; 84 | gmtime_r(&tDate,&tmDate); 85 | converted_value = tmDate; 86 | cresult = true; 87 | break; 88 | } 89 | } 90 | while(false); 91 | if(cresult) 92 | { 93 | result[name] = converted_value; 94 | } 95 | } 96 | return result; 97 | } 98 | 99 | -------------------------------------------------------------------------------- /ros_ads_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ros_ads_node) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") 7 | 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | 10 | set(CMAKE_CXX_EXTENSIONS OFF) 11 | 12 | find_package(Threads REQUIRED) 13 | set(CMAKE_BUILD_TYPE debug) 14 | 15 | add_subdirectory(lib/ADS/AdsLib) 16 | ## Find catkin macros and libraries 17 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 18 | ## is used, also find other catkin packages 19 | find_package(catkin REQUIRED COMPONENTS 20 | cmake_modules 21 | roscpp 22 | roslib 23 | std_msgs 24 | message_generation 25 | ros_ads_msgs 26 | ) 27 | find_package(TinyXML REQUIRED) 28 | find_library(yaml-cpp REQUIRED) 29 | 30 | catkin_package( 31 | LIBRARIES 32 | CATKIN_DEPENDS roscpp std_msgs roslib 33 | # DEPENDS system_lib 34 | ) 35 | 36 | 37 | 38 | include_directories( 39 | include 40 | ${catkin_INCLUDE_DIRS} 41 | ${TinyXML_INCLUDE_DIRS} 42 | ) 43 | 44 | 45 | 46 | add_executable(ros_ads_node src/Ads_Interface.cpp src/Ads_node.cpp) 47 | add_dependencies(ros_ads_node ${catkin_EXPORTED_TARGETS} ) 48 | add_dependencies(ros_ads_node ads) 49 | set_property(TARGET ros_ads_node PROPERTY CXX_STANDARD 17) 50 | set_property(TARGET ros_ads_node PROPERTY CXX_STANDARD_REQUIRED ON) 51 | 52 | target_link_libraries(ros_ads_node ${catkin_LIBRARIES} ${TinyXML_LIBRARIES} yaml-cpp ads -lpthread ) 53 | 54 | ## Mark executables for installation 55 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 56 | # install(TARGETS ${PROJECT_NAME}_node 57 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 58 | # ) 59 | 60 | install(TARGETS ros_ads_node 61 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 64 | ) 65 | 66 | ## Mark other files for installation (e.g. launch and bag files, etc.) 67 | install( 68 | DIRECTORY launch/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 70 | PATTERN ".svn" EXCLUDE) 71 | install( 72 | DIRECTORY config/ 73 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 74 | PATTERN ".svn" EXCLUDE) 75 | -------------------------------------------------------------------------------- /ros_ads_node/config/configuration.yaml: -------------------------------------------------------------------------------- 1 | #automat map 2 | 3 | test_device: 4 | 5 | remoteIP: "172.21.176.50" 6 | remoteNetID: "172.18.236.170.1.1" 7 | localNetID: "172.21.176.10.1.1" 8 | publish_rate: 1 #Hz 9 | refresh_rate: 10 #Hz 10 | state_rate: 1 #Hz 11 | 12 | publish_on_timer: 13 | - alias1 14 | - alias2 15 | 16 | publish_on_event: 17 | - alias3 18 | 19 | variables: 20 | #GVL_ROS must be replaced by your variable file, GVL_ROS is recommanded for readability 21 | GVL_ROS.MySuperVariable1: alias1 22 | GVL_ROS.MySuperVariable2: alias2 23 | GVL_ROS.MySuperVariable3: alias3 24 | GVL_ROS.MySuperVariable4: alias4 25 | -------------------------------------------------------------------------------- /ros_ads_node/launch/ADSnode.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ros_ads_node/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ros_ads_node 4 | 0.0.0 5 | The plc_control package 6 | 7 | elvin 8 | BSD 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | catkin 29 | roscpp 30 | roslib 31 | std_msgs 32 | message_generation 33 | tinyxml 34 | cmake_modules 35 | ros_ads_msgs 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /ros_ads_node/src/Ads_Interface.cpp: -------------------------------------------------------------------------------- 1 | #include "Ads_Interface.h" 2 | 3 | using namespace std; 4 | 5 | /** 6 | * @brief RosAds_Interface simple consctructor 7 | */ 8 | RosAds_Interface::RosAds_Interface() 9 | { 10 | } 11 | 12 | RosAds_Interface::~RosAds_Interface() 13 | { 14 | if(m_route) 15 | { 16 | delete m_route; 17 | } 18 | if(m_AmsNetIdremoteNetId) 19 | { 20 | delete m_AmsNetIdremoteNetId; 21 | } 22 | 23 | for(map::iterator it = m_RouteMapping.begin(); it != m_RouteMapping.end(); ++it) 24 | { 25 | delete it->second; 26 | } 27 | } 28 | 29 | /** 30 | * @brief adsWriteValue writes on a single variable 31 | * @param name name of the variable to write on 32 | * @param value value of the variable to write on 33 | * @return true if the writing succeded 34 | * @return false otherwise 35 | */ 36 | bool RosAds_Interface::adsWriteValue(string name, variant_t value){ 37 | 38 | int varType = checkVariableType(name); 39 | bool dataCorrect = true; 40 | bool bresult = true; 41 | auto no_issue = true; 42 | if( m_VariableMapping.find(name) == m_VariableMapping.end()){ 43 | dataCorrect = false; 44 | bresult = false; 45 | } 46 | else if (m_VariableMapping[name].first == varType && m_device_state) 47 | { 48 | m_ComMutex.lock(); 49 | m_MemMutex.lock(); 50 | try 51 | { 52 | switch(m_VariableMapping[name].first) 53 | { 54 | case BOOL: 55 | { 56 | *m_RouteMapping[name] = get(value); 57 | break; 58 | } 59 | case UINT8_T: 60 | { 61 | *m_RouteMapping[name] = get(value); 62 | break; 63 | } 64 | case INT8_T: 65 | { 66 | *m_RouteMapping[name] = get(value); 67 | break; 68 | } 69 | case UINT16_T: 70 | { 71 | *m_RouteMapping[name] = get(value); 72 | break; 73 | } 74 | case INT16_T: 75 | { 76 | *m_RouteMapping[name] = get(value); 77 | break; 78 | } 79 | case UINT32_T: 80 | { 81 | *m_RouteMapping[name] = get(value); 82 | break; 83 | } 84 | case INT32_T: 85 | { 86 | *m_RouteMapping[name] = get(value); 87 | break; 88 | } 89 | case INT64_T: 90 | { 91 | *m_RouteMapping[name] = get(value); 92 | break; 93 | } 94 | case FLOAT: 95 | { 96 | *m_RouteMapping[name] = get(value); 97 | break; 98 | } 99 | case DOUBLE: 100 | { 101 | *m_RouteMapping[name] = get(value); 102 | break; 103 | } 104 | case DATE: 105 | { 106 | tm temp = get(value); 107 | *m_RouteMapping[name] = mktime(&temp); 108 | break; 109 | } 110 | default: 111 | { 112 | no_issue = false; 113 | } 114 | } 115 | } 116 | catch(AdsException e) 117 | { 118 | no_issue = false; 119 | } 120 | m_MemMutex.unlock(); 121 | m_ComMutex.unlock(); 122 | } 123 | else 124 | { 125 | factory(name); 126 | } 127 | if(!no_issue){ 128 | factory(name); 129 | } 130 | 131 | if(!dataCorrect) { 132 | bresult = false; 133 | } 134 | return bresult; 135 | } 136 | 137 | /** 138 | * @brief adsReadValue reads a single variable's value 139 | * @param varName name of the variable to read 140 | * @return variant_t value of the variable 141 | */ 142 | RosAds_Interface::variant_t RosAds_Interface::adsReadValue(string name) 143 | { 144 | variant_t result; 145 | 146 | if(m_RouteMapping.find(name) != m_RouteMapping.end()) 147 | { 148 | auto no_issue = true; 149 | m_ComMutex.lock(); 150 | m_MemMutex.lock(); 151 | if(m_device_state) 152 | { 153 | try 154 | { 155 | if(m_RouteMapping[name]) 156 | { 157 | void* value =(void*)&m_temp; 158 | m_RouteMapping[name]->ReadValue(value); 159 | switch(m_VariableMapping[name].first) 160 | { 161 | case BOOL: 162 | { 163 | result =*(bool*)value; 164 | break; 165 | } 166 | case UINT8_T: 167 | { 168 | result = *(uint8_t*)value; 169 | break; 170 | } 171 | case INT8_T: 172 | { 173 | result = *(int8_t*)value; 174 | break; 175 | } 176 | case UINT16_T: 177 | { 178 | result = *(uint16_t*)value; 179 | break; 180 | } 181 | case INT16_T: 182 | { 183 | result = *(int16_t*)value; 184 | break; 185 | } 186 | case UINT32_T: 187 | { 188 | result = *(uint32_t*)value; 189 | break; 190 | } 191 | case INT32_T: 192 | { 193 | result = *(int32_t*)value; 194 | break; 195 | } 196 | case INT64_T: 197 | { 198 | result = *(int64_t*)value; 199 | break; 200 | } 201 | case FLOAT: 202 | { 203 | result = *(float*)value; 204 | break; 205 | } 206 | case DOUBLE: 207 | { 208 | result = *(double*)value; 209 | break; 210 | } 211 | case DATE: 212 | { 213 | result = *(uint32_t*)value; 214 | break; 215 | } 216 | default: 217 | { 218 | 219 | } 220 | } 221 | } 222 | else 223 | { 224 | no_issue = false; 225 | } 226 | } 227 | catch(...) 228 | { 229 | no_issue = false; 230 | } 231 | } 232 | m_MemMutex.unlock(); 233 | m_ComMutex.unlock(); 234 | if(!no_issue) 235 | { 236 | factory(name); 237 | } 238 | } 239 | return result; 240 | } 241 | 242 | /** 243 | * @brief adsReadVariables reads multiple variables' value 244 | * @param varNames names of the variables to read 245 | * @return vector values of the variables 246 | */ 247 | vector RosAds_Interface::adsReadVariables(vector varNames) 248 | { 249 | vector result; 250 | 251 | for (auto& name: varNames) 252 | { 253 | result.push_back(adsReadValue(name)); 254 | } 255 | 256 | return result; 257 | } 258 | 259 | /** 260 | * @brief factory (re)-create an IADS variable 261 | * @param varName the alias of the variable to (re)-create 262 | * @return true if the creation succeeded 263 | */ 264 | bool RosAds_Interface::factory(string varName) 265 | { 266 | bool result = false; 267 | auto no_issue = true; 268 | m_MemMutex.lock(); 269 | try { 270 | string type = m_VariableADS[m_Alias_map[varName]]; 271 | do{ 272 | if(m_RouteMapping[varName]) 273 | { 274 | delete m_RouteMapping[varName]; 275 | } 276 | if(type == "BOOL"){ 277 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 278 | result = true; 279 | break; 280 | } 281 | if(type == "BYTE" || type == "USINT"){ 282 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 283 | result = true; 284 | break; 285 | } 286 | if(type == "SINT"){ 287 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 288 | result = true; 289 | break; 290 | } 291 | if(type == "WORD" || type == "UINT"){ 292 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 293 | result = true; 294 | break; 295 | } 296 | if(type == "INT"){ 297 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 298 | result = true; 299 | break; 300 | } 301 | if(type == "DWORD" || type == "UDINT" || type == "DATE" || type == "TIME" || type == "TIME_OF_DAY" || type == "LTIME"){ 302 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 303 | result = true; 304 | break; 305 | } 306 | if(type == "DINT"){ 307 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 308 | result = true; 309 | break; 310 | } 311 | if(type == "LINT"){ 312 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 313 | result = true; 314 | break; 315 | } 316 | if(type == "REAL"){ 317 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 318 | result = true; 319 | break; 320 | } 321 | if(type == "LREAL"){ 322 | m_RouteMapping[varName] = new AdsVariable(*m_route, m_Alias_map[varName]); 323 | result = true; 324 | break; 325 | } 326 | } 327 | while(false); 328 | } catch (...) { 329 | no_issue = false; 330 | } 331 | m_MemMutex.unlock(); 332 | 333 | if(!no_issue){ 334 | m_ads_state = false; 335 | connectionCheck(); 336 | } 337 | 338 | return result; 339 | } 340 | 341 | /** 342 | * @brief convert_type_from_string gets the type from a string 343 | * @param type the type as string 344 | * @return the type as int 345 | * @return -1 if not a type 346 | */ 347 | int RosAds_Interface::convert_type_from_string(string type) 348 | { 349 | int result = -1; 350 | do 351 | { 352 | if(type == "BOOL") 353 | { 354 | result = BOOL; 355 | break; 356 | } 357 | if(type == "BYTE" || type == "USINT") 358 | { 359 | result = UINT8_T; 360 | break; 361 | } 362 | if(type == "SINT") 363 | { 364 | result = INT8_T; 365 | break; 366 | } 367 | if(type == "WORD" || type == "UINT") 368 | { 369 | result = UINT16_T; 370 | break; 371 | } 372 | if(type == "INT") 373 | { 374 | result = INT16_T; 375 | break; 376 | } 377 | if(type == "DWORD" || type == "UDINT") 378 | { 379 | result = UINT32_T; 380 | break; 381 | } 382 | if(type == "DINT") 383 | { 384 | result = INT32_T; 385 | break; 386 | } 387 | if(type == "LINT") 388 | { 389 | result = INT64_T; 390 | break; 391 | } 392 | if(type == "REAL") 393 | { 394 | result = FLOAT; 395 | break; 396 | } 397 | if(type == "LREAL") 398 | { 399 | result = DOUBLE; 400 | break; 401 | } 402 | if(type == "DATE") 403 | { 404 | result = DATE; 405 | break; 406 | } 407 | } 408 | while(false); 409 | return result; 410 | } 411 | 412 | /** 413 | * @brief updateMemory update the variables memory 414 | */ 415 | void RosAds_Interface::updateMemory() 416 | { 417 | for(auto &[name, pair]: m_variables_map) 418 | { 419 | pair.second = adsReadValue(name); 420 | } 421 | } 422 | 423 | /** 424 | * @brief initRoute initialize a connection to the ADS device 425 | */ 426 | void RosAds_Interface::initRoute() 427 | { 428 | m_AmsNetIdremoteNetId= new AmsNetId(m_remoteNetId); 429 | m_route = new AdsDevice(m_remoteIpV4.c_str(),*m_AmsNetIdremoteNetId, AMSPORT_R0_PLC_TC3); 430 | } 431 | 432 | /** 433 | * @brief connectionCheck 434 | * 435 | * Tries to get ADS state 436 | * If the ADS is not running setsm_device state to false, to true otherwise 437 | * If the ADS is not running or connection failed, tries to establish a new connection 438 | * If connection was down and is up again, recreates all IADS variables (get the new handles) 439 | * 440 | * @return ads state 441 | */ 442 | int RosAds_Interface::connectionCheck() 443 | { 444 | auto result = false; 445 | AdsDeviceState test; 446 | auto temp_state = m_device_state; 447 | m_ComMutex.lock(); 448 | try { 449 | test = m_route->GetState(); 450 | result = (test.ads == ADSSTATE_RUN); 451 | m_ads_state = (uint16_t)test.ads; 452 | } catch (...) { 453 | m_ads_state = ADSSTATE_INVALID; 454 | } 455 | 456 | if (!result) //recovery 457 | { 458 | if(m_route) 459 | { 460 | delete m_route; 461 | } 462 | if(m_AmsNetIdremoteNetId) 463 | { 464 | delete m_AmsNetIdremoteNetId; 465 | } 466 | initRoute(); 467 | } 468 | m_ComMutex.unlock(); 469 | 470 | if(result && !temp_state) //recreate ADSVariables if connexion is re-established 471 | { 472 | acquireVariables(); 473 | for(auto &[name, alias]: m_VariableMapping) 474 | { 475 | factory(name); 476 | } 477 | } 478 | 479 | m_device_state = result; 480 | 481 | return (int)test.ads; 482 | } 483 | 484 | /** 485 | * @brief bindPLcVar creates IADS variables for aliased variables given in configuration file 486 | * @return true if aliasing succeeded 487 | * @return false otherwise 488 | */ 489 | bool RosAds_Interface::bindPLcVar() 490 | { 491 | bool bresult = false; 492 | YAML::Node config = YAML::LoadFile(m_config_file); 493 | if (config[m_name]) 494 | { 495 | 496 | //Read each alias with corresponding ADS name 497 | for(YAML::const_iterator element=config[m_name]["variables"].begin();element!=config[m_name]["variables"].end();++element) 498 | { 499 | string adsName = element->first.as(); 500 | string alias = element->second.as(); 501 | //Check if ADS name is part of downloaded PLC ADS list 502 | if ( m_VariableADS.find(adsName) == m_VariableADS.end() ) 503 | { 504 | continue; 505 | } 506 | 507 | string type = m_VariableADS[adsName]; 508 | m_VariableMapping[alias] = pair(convert_type_from_string(type), type); 509 | m_variables_map[alias] = pair(type, variant_t()); 510 | m_Alias_map[alias] = adsName; 511 | factory(alias); 512 | } 513 | bresult = true; 514 | } 515 | else 516 | { 517 | 518 | } 519 | return bresult; 520 | } 521 | 522 | /** 523 | * @brief checkVariableType 524 | * @param varName the name of the variable to check the type of 525 | * @return the type of the variable as int 526 | * @return -1 if variable does not exist 527 | */ 528 | int RosAds_Interface::checkVariableType(string varName){ 529 | int varType = -1; 530 | map>::iterator it; 531 | it= m_VariableMapping.find(varName); 532 | if(it != m_VariableMapping.end()) 533 | { 534 | varType = it->second.first; 535 | } 536 | return varType; 537 | } 538 | -------------------------------------------------------------------------------- /ros_ads_node/src/Ads_Interface.h: -------------------------------------------------------------------------------- 1 | #ifndef HEADER_H_ADS_INTERFACE 2 | #define HEADER_H_ADS_INTERFACE 3 | 4 | #include 5 | #include 6 | #include "../lib/ADS/AdsLib/standalone/AdsDef.h" 7 | #include "../lib/ADS/AdsLib/AdsLib.h" 8 | #include "../lib/ADS/AdsLib/AdsVariable.h" 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | class RosAds_Interface 17 | { 18 | enum{ 19 | BOOL, 20 | UINT8_T, 21 | INT8_T, 22 | UINT16_T, 23 | INT16_T, 24 | UINT32_T, 25 | INT32_T, 26 | INT64_T, 27 | FLOAT, 28 | DOUBLE, 29 | DATE, 30 | }; 31 | 32 | public : 33 | 34 | using variant_t = variant; 35 | 36 | /** 37 | * @brief RosAds_Interface simple consctructor 38 | */ 39 | RosAds_Interface(); 40 | 41 | ~RosAds_Interface(); 42 | 43 | /** 44 | * @brief adsWriteValue writes on a single variable 45 | * @param name name of the variable to write on 46 | * @param value value of the variable to write on 47 | * @return true if the writing succeded 48 | * @return false otherwise 49 | */ 50 | bool adsWriteValue(string name, variant_t value); 51 | 52 | /** 53 | * @brief adsReadValue reads a single variable's value 54 | * @param varName name of the variable to read 55 | * @return variant_t value of the variable 56 | */ 57 | variant_t adsReadValue(string varName); 58 | 59 | /** 60 | * @brief adsReadVariables reads multiple variables' value 61 | * @param varNames names of the variables to read 62 | * @return vector values of the variables 63 | */ 64 | vector adsReadVariables(vector varNames); 65 | 66 | /** 67 | * @brief factory (re)-create an IADS variable 68 | * @param varName the alias of the variable to (re)-create 69 | * @return true if the creation succeeded 70 | */ 71 | bool factory(string varName); 72 | 73 | /** 74 | * @brief convert_type_from_string gets the type from a string 75 | * @param type the type as string 76 | * @return the type as int 77 | * @return -1 if not a type 78 | */ 79 | int convert_type_from_string(string type); 80 | 81 | /** 82 | * @brief setRemoteNetID set the remote NetID 83 | * @param netId netID to set as string of type: xxx.xxx.xxx.xxx.xxx.xxx 84 | */ 85 | void setRemoteNetID(string netId){m_remoteNetId = netId;} 86 | 87 | /** 88 | * @brief getRemoteNetID get the remote NetID 89 | * @return netId netID parameter 90 | */ 91 | string getRemoteNetID(){return m_remoteNetId;} 92 | 93 | /** 94 | * @brief setRemoteIPV4 set the remoteIPV4 95 | * @param address IPV4 to set as string of type: xxx.xxx.xxx.xxx 96 | */ 97 | void setRemoteIPV4(string address){m_remoteIpV4 = address;} 98 | 99 | /** 100 | * @brief getRemoteIPV4 get the remote IPV4 101 | * @return IPV4 IPV4 parameter 102 | */ 103 | string getRemoteIPV4(){return m_remoteIpV4;} 104 | 105 | /** 106 | * @brief setLocalNetID set the local NetID 107 | * @param netId netID to set as string of type: xxx.xxx.xxx.xxx.xxx.xxx 108 | */ 109 | void setLocalNetID(string netId){m_localNetId_param = netId; 110 | AdsSetLocalAddress(AmsNetId(m_localNetId_param));} 111 | 112 | /** 113 | * @brief getLocalNetID get the local NetID 114 | * @return netId netID parameter 115 | */ 116 | string getLocalNetID(){return m_localNetId_param;} 117 | 118 | /** 119 | * @brief getVariablesMap 120 | * @return a copy of the variables memory 121 | */ 122 | map> getVariablesMap(){return m_variables_map;} 123 | 124 | /** 125 | * @brief updateMemory update the variables memory 126 | */ 127 | void updateMemory(); 128 | 129 | /** 130 | * @brief initRoute initialize a connection to the ADS device 131 | */ 132 | void initRoute(); 133 | 134 | /** 135 | * @brief connectionCheck 136 | * 137 | * Tries to get ADS state 138 | * If the ADS is not running setsm_device state to false, to true otherwise 139 | * If the ADS is not running or connection failed, tries to establish a new connection 140 | * If connection was down and is up again, recreates all IADS variables (get the new handles) 141 | * 142 | * @return ads state 143 | */ 144 | int connectionCheck(); 145 | 146 | /** 147 | * @brief getState 148 | * @return true if data published are valid 149 | * @return false otherwise 150 | */ 151 | bool getState(){return(m_device_state);} 152 | 153 | /** 154 | * @brief getADSState 155 | * @return return current ADS state 156 | */ 157 | int getADSState(){return(m_ads_state);} 158 | 159 | /** 160 | * @brief bindPLcVar creates IADS variables for aliased variables given in configuration file 161 | * @return true if aliasing succeeded 162 | * @return false otherwise 163 | */ 164 | bool bindPLcVar(); 165 | 166 | /** 167 | * @brief checkVariableType 168 | * @param varName the name of the variable to check the type of 169 | * @return the type of the variable as int 170 | * @return -1 if variable does not exist 171 | */ 172 | int checkVariableType(string varName); 173 | 174 | /** 175 | * @brief acquireVariables get all ADS variables from the device 176 | */ 177 | void acquireVariables(){m_VariableADS = m_route->GetDeviceAdsVariables();} 178 | 179 | /** 180 | * @brief setName set the name of the device for configuration 181 | * @param name the name of the device 182 | */ 183 | void setName(string name){m_name = name;} 184 | 185 | /** 186 | * @brief setFile set the configuration file the device is configured from 187 | * @param config_file the full path to the YAML configuration file 188 | */ 189 | void setFile(string config_file){m_config_file = config_file;} 190 | 191 | 192 | private: 193 | 194 | string m_remoteNetId; /*!< the NetID of the ADS device */ 195 | string m_remoteIpV4; /*!< the IPV4 of the ADS device */ 196 | string m_localNetId_param; /*!< the local net ID */ 197 | double m_temp; /*!< a temp value for reading a variable value */ 198 | string m_name; /*!< the name of the device for configuration */ 199 | string m_config_file; /*!< the configuration file to use */ 200 | int m_ads_state; /*!< the last known state of ADS */ 201 | 202 | AmsNetId* m_AmsNetIdremoteNetId; /*!< the NetID structure of the ADS device */ 203 | AdsDevice* m_route; /*!< the ADS device route */ 204 | bool m_device_state{true}; /*!< the last known validity state of the values */ 205 | 206 | 207 | //Mutex utile pour la communication 208 | mutex m_ComMutex; /*!< Communication mutex */ 209 | mutex m_MemMutex; /*!< memory mutex */ 210 | 211 | map m_VariableADS; /*!< a map with ADS name as key and the variable type as value */ 212 | map m_Alias_map; /*!< a map with alias name as key and ADS names as value */ 213 | map> m_VariableMapping; /*!< a map with alias name as key and both representation of it's type a value */ 214 | map m_RouteMapping; /*!< a map with alias name as key and the IADS variable as value */ 215 | map> m_variables_map; /*!< a map with alias name as key and a pair with the type of the variable as string and it's value in it's own type */ 216 | }; 217 | #endif 218 | -------------------------------------------------------------------------------- /ros_ads_node/src/Ads_node.cpp: -------------------------------------------------------------------------------- 1 | #include "Ads_node.h" 2 | 3 | #include 4 | #include 5 | 6 | using namespace ads_node; 7 | 8 | /** 9 | * @brief ADSNode::initialize initialize the parameters of the node and it's variables 10 | * @return 1 if the initialization failed 11 | * @return 0 otherwise 12 | */ 13 | bool ADSNode::initialize() 14 | { 15 | ros::NodeHandle n; 16 | ros::NodeHandle nprive("~"); 17 | 18 | if(nprive.hasParam("name")) 19 | { 20 | nprive.getParam("name", m_name); 21 | } 22 | else 23 | { 24 | ROS_ERROR("Param name unknown"); 25 | return 0; 26 | } 27 | 28 | if(nprive.hasParam("config_file")) 29 | { 30 | try { //get parameters from YAML file 31 | nprive.getParam("config_file", m_config_file); 32 | YAML::Node config = YAML::LoadFile(m_config_file); 33 | if(config[m_name]) 34 | { 35 | m_ADS.setLocalNetID(config[m_name]["localNetID"].as()); 36 | m_ADS.setRemoteNetID(config[m_name]["remoteNetID"].as()); 37 | m_ADS.setRemoteIPV4(config[m_name]["remoteIP"].as()); 38 | m_rate_update = config[m_name]["refresh_rate"].as(); 39 | m_rate_publish = config[m_name]["publish_rate"].as(); 40 | m_rate_state = config[m_name]["state_rate"].as(); 41 | 42 | if(config[m_name]["publish_on_timer"].size() != 0) 43 | { 44 | auto onTimer = config[m_name]["publish_on_timer"].as>(); 45 | for(auto & var: onTimer) 46 | { 47 | m_publish_on_timer_guard.lock(); 48 | m_publish_on_timer[var] = pair(); 49 | m_publish_on_timer_guard.unlock(); 50 | m_variables_guard.lock(); 51 | m_variables[var] = pair(); 52 | m_variables_guard.unlock(); 53 | } 54 | } 55 | 56 | if(config[m_name]["publish_on_event"].size() != 0) 57 | { 58 | auto onEvent = config[m_name]["publish_on_event"].as>(); 59 | for(auto & var: onEvent) 60 | { 61 | m_publish_on_event_guard.lock(); 62 | m_publish_on_event[var] = pair(); 63 | m_publish_on_event_guard.unlock(); 64 | m_variables_guard.lock(); 65 | m_variables[var] = pair(); 66 | m_variables_guard.unlock(); 67 | } 68 | } 69 | } 70 | } catch (...) 71 | { 72 | ROS_ERROR("Invalid configuration file"); 73 | } 74 | 75 | } 76 | else { 77 | ROS_ERROR("Error with configuration file"); 78 | return 0; 79 | } 80 | 81 | try 82 | { 83 | ROS_INFO_STREAM("GO FOR Init Route"); 84 | m_ADS.initRoute(); 85 | ROS_INFO("Init Route done"); 86 | } 87 | catch(...) 88 | { 89 | ROS_ERROR("ERROR while connecting with ADS"); 90 | return 0; 91 | } 92 | 93 | try 94 | { 95 | ROS_INFO("Acquiring ADS variables"); 96 | m_ADS.acquireVariables(); 97 | ROS_INFO("ADS variables acquired"); 98 | 99 | ROS_INFO("Aliasing ADS variables"); 100 | m_ADS.setName(m_name); 101 | m_ADS.setFile(m_config_file); 102 | m_ADS.bindPLcVar(); 103 | ROS_INFO("Ready to communicate with the remote PLC via ADS."); 104 | } 105 | catch(AdsException e) 106 | { 107 | ROS_ERROR_STREAM(e.what()); 108 | ROS_ERROR("ERROR in mapping alias with ADS"); 109 | return 0; 110 | } 111 | 112 | try 113 | { 114 | m_subscriber = n.subscribe("command", 10, boost::bind(&ADSNode::Subscriber, this, _1)); 115 | m_on_event_publisher = n.advertise("report_event", 10); 116 | m_on_timer_publisher = n.advertise("report_timer", 10); 117 | m_state_publisher = n.advertise("state", 10); 118 | } 119 | catch (...) 120 | { 121 | ROS_ERROR("ERROR in creating subscriber"); 122 | return 0; 123 | } 124 | 125 | m_timer_thread = make_shared(&ADSNode::publishOnTimer,this, m_rate_publish); 126 | m_update_thread = make_shared(&ADSNode::update,this, m_rate_update); 127 | m_checker_thread = make_shared(&ADSNode::publishOnEvent,this, m_rate_update); 128 | m_state_thread = make_shared(&ADSNode::publishState,this, m_rate_state); 129 | 130 | return 1; 131 | } 132 | 133 | /** 134 | * @brief ADSNode::update update internal variable values memory 135 | * 136 | * Starts by verifying and updating connection state 137 | * 138 | * @param timer_rate rate to update at 139 | */ 140 | void ADSNode::update(int timer_rate) 141 | { 142 | auto loop_rate_update = ros::Rate(timer_rate); 143 | while(ros::ok()) 144 | { 145 | m_variables_guard.lock(); 146 | try 147 | { 148 | m_ADS.connectionCheck(); 149 | if(m_ADS.getState()) 150 | { 151 | m_ADS.updateMemory(); 152 | auto variables_map = m_ADS.getVariablesMap(); 153 | for(auto &[name, pair]: variables_map) 154 | { //cast as double for the message 155 | m_variables[name].first = pair.first; 156 | switch (pair.second.index()) 157 | { 158 | case ros_ads_msgs::BOOL: 159 | { 160 | m_variables[name].second = static_cast(get(pair.second)); 161 | break; 162 | } 163 | case ros_ads_msgs::UINT8_T: 164 | { 165 | m_variables[name].second = static_cast(get(pair.second)); 166 | break; 167 | } 168 | case ros_ads_msgs::INT8_T: 169 | { 170 | m_variables[name].second = static_cast(get(pair.second)); 171 | break; 172 | } 173 | case ros_ads_msgs::UINT16_T: 174 | { 175 | m_variables[name].second = static_cast(get(pair.second)); 176 | break; 177 | } 178 | case ros_ads_msgs::INT16_T: 179 | { 180 | m_variables[name].second = static_cast(get(pair.second)); 181 | break; 182 | } 183 | case ros_ads_msgs::UINT32_T: 184 | { 185 | m_variables[name].second = static_cast(get(pair.second)); 186 | break; 187 | } 188 | case ros_ads_msgs::INT32_T: 189 | { 190 | m_variables[name].second = static_cast(get(pair.second)); 191 | break; 192 | } 193 | case ros_ads_msgs::INT64_T: 194 | { 195 | m_variables[name].second = static_cast(get(pair.second)); 196 | break; 197 | } 198 | case ros_ads_msgs::FLOAT: 199 | { 200 | m_variables[name].second = static_cast(get(pair.second)); 201 | break; 202 | } 203 | case ros_ads_msgs::DOUBLE: 204 | { 205 | m_variables[name].second = static_cast(get(pair.second)); 206 | break; 207 | } 208 | case ros_ads_msgs::DATE: 209 | { 210 | tm temp = get(pair.second); 211 | m_variables[name].second = static_cast(mktime(&temp)); 212 | break; 213 | } 214 | default: 215 | { 216 | 217 | } 218 | } 219 | m_publish_on_timer_guard.lock(); 220 | if(m_publish_on_timer.find(name) != m_publish_on_timer.end() && m_ADS.getState()) 221 | { 222 | m_publish_on_timer[name] = m_variables[name]; 223 | } 224 | m_publish_on_timer_guard.unlock(); 225 | } 226 | } 227 | } 228 | catch (...) 229 | { 230 | ROS_ERROR("fail timer CB"); 231 | } 232 | m_variables_guard.unlock(); 233 | loop_rate_update.sleep(); 234 | } 235 | } 236 | 237 | /** 238 | * @brief ADSNode::publishState publish state periodically 239 | * @param timer_rate rate to publish at 240 | */ 241 | void ADSNode::publishState(int timer_rate) 242 | { 243 | auto loop_rate_state = ros::Rate(timer_rate); 244 | while(ros::ok()) 245 | { 246 | m_state_msg.header.stamp = ros::Time::now(); 247 | m_state_msg.state = m_ADS.getState(); 248 | m_state_msg.error = m_ADS.getADSState(); 249 | m_state_publisher.publish(m_state_msg); 250 | loop_rate_state.sleep(); 251 | } 252 | } 253 | 254 | /** 255 | * @brief ADSNode::publishOnEvent publish variables if an event occured 256 | * @param timer_rate rate to verify if an event occured at 257 | */ 258 | void ADSNode::publishOnEvent(int timer_rate) 259 | { 260 | auto loop_rate_check = ros::Rate(timer_rate); 261 | while(ros::ok()) 262 | { 263 | try 264 | { 265 | if(m_ADS.getState()) 266 | { 267 | m_publish_on_event_guard.lock(); 268 | auto publish_on_event = m_publish_on_event; 269 | m_publish_on_event_guard.unlock(); 270 | m_variables_guard.lock(); 271 | auto variables = m_variables; 272 | m_variables_guard.unlock(); 273 | 274 | m_publish = false; 275 | for(auto &[key, pair_] : publish_on_event) 276 | { 277 | m_checker_temp_value = variables[key]; 278 | if(pair_ != m_checker_temp_value) // A change has occured 279 | { 280 | m_publish_on_event_guard.lock(); 281 | m_publish_on_event[key] = m_checker_temp_value; // Update value 282 | m_publish_on_event_guard.unlock(); 283 | 284 | if(!m_publish) 285 | { 286 | m_publish = true; 287 | } 288 | } 289 | } 290 | if (m_publish) // Publish if a change occured 291 | { 292 | m_on_event_msg.header.stamp = ros::Time::now(); 293 | m_on_event_msg.varNames = vector(); 294 | m_on_event_msg.varTypes = vector(); 295 | m_on_event_msg.varValues = vector(); 296 | 297 | for (auto &[key, pair] : publish_on_event) 298 | { 299 | m_on_event_msg.varNames.push_back(key); 300 | auto type = pair.first; 301 | m_on_event_msg.varTypes.push_back(type); 302 | auto value = pair.second; 303 | m_on_event_msg.varValues.push_back(value); 304 | } 305 | if(m_on_event_msg.varNames.size() !=0) 306 | { 307 | m_on_event_publisher.publish(m_on_event_msg); 308 | } 309 | } 310 | } 311 | } 312 | catch(...) 313 | { 314 | ROS_ERROR("fail checker CB"); 315 | } 316 | loop_rate_check.sleep(); 317 | } 318 | 319 | } 320 | 321 | /** 322 | * @brief ADSNode::publishOnTimer publish variables periodically 323 | * @param timer_rate rate to publish at 324 | */ 325 | void ADSNode::publishOnTimer(int timer_rate) 326 | { 327 | 328 | auto loop_rate_pub = ros::Rate(timer_rate); 329 | while(ros::ok()) 330 | { 331 | try 332 | { 333 | m_on_timer_msg.header.stamp = ros::Time::now(); 334 | m_on_timer_msg.varNames = vector(); 335 | m_on_timer_msg.varTypes = vector(); 336 | m_on_timer_msg.varValues = vector(); 337 | 338 | m_publish_on_timer_guard.lock(); 339 | auto publish_on_timer = m_publish_on_timer; 340 | m_publish_on_timer_guard.unlock(); 341 | 342 | for(auto &[name, pair]: publish_on_timer) 343 | { 344 | m_on_timer_msg.varNames.push_back(name); 345 | m_on_timer_msg.varTypes.push_back(pair.first); 346 | m_on_timer_msg.varValues.push_back(pair.second); 347 | } 348 | 349 | 350 | if(m_on_timer_msg.varNames.size() != 0) 351 | { 352 | m_on_timer_publisher.publish(m_on_timer_msg); 353 | } 354 | loop_rate_pub.sleep(); 355 | } 356 | catch(...) 357 | { 358 | ROS_ERROR("fail pub CB"); 359 | } 360 | 361 | } 362 | } 363 | 364 | /** 365 | * @brief ADSNode::Subscriber send the ADS device the received commands 366 | * @param msg the command message 367 | * @return true if the command was successfully sent 368 | */ 369 | bool ADSNode::Subscriber(const ros_ads_msgs::ADS::ConstPtr &msg) 370 | { 371 | m_state_publisher.publish(m_state_msg); 372 | bool result = true; 373 | auto command = ros_ads_msgs::decode(msg); 374 | 375 | try { 376 | if(m_ADS.getState()) 377 | { 378 | for (auto &[name, value]: command) 379 | { 380 | m_ADS.adsWriteValue(name, value); 381 | } 382 | } 383 | } 384 | catch (...) 385 | { 386 | result = false; 387 | } 388 | 389 | return result; 390 | } 391 | 392 | int main(int argc, char **argv) 393 | { 394 | ros::init(argc, argv, "ros_ads_node"); 395 | ADSNode *node = new ADSNode(); 396 | 397 | node->initialize(); 398 | 399 | while(ros::ok()) 400 | { 401 | ros::spinOnce(); 402 | } 403 | return 0; 404 | } 405 | -------------------------------------------------------------------------------- /ros_ads_node/src/Ads_node.h: -------------------------------------------------------------------------------- 1 | #ifndef HEADER_H_ADS_NODE 2 | #define HEADER_H_ADS_NODE 3 | 4 | //YAML include 5 | #include 6 | 7 | #include "ros/ros.h" 8 | #include "ros/package.h" 9 | #include 10 | #include "Ads_Interface.h" 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | //Standard includes 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | using namespace std::chrono_literals; 27 | using namespace std; 28 | 29 | namespace ads_node { 30 | 31 | class ADSNode 32 | { 33 | 34 | public: 35 | 36 | /** 37 | * @brief ADSNode::initialize initialize the parameters of the node and it's variables 38 | * @return 1 if the initialization failed 39 | * @return 0 otherwise 40 | */ 41 | bool initialize(); 42 | 43 | /** 44 | * @brief ADSNode::update update internal variable values memory 45 | * 46 | * Starts by verifying and updating connection state 47 | * 48 | * @param timer_rate rate to update at 49 | */ 50 | void update(int timer_rate); 51 | 52 | /** 53 | * @brief ADSNode::publishState publish state periodically 54 | * @param timer_rate rate to publish at 55 | */ 56 | void publishState(int timer_rate); 57 | 58 | /** 59 | * @brief ADSNode::publishOnEvent publish variables if an event occured 60 | * @param timer_rate rate to verify if an event occured at 61 | */ 62 | void publishOnEvent(int timer_rate); 63 | 64 | /** 65 | * @brief ADSNode::publishOnTimer publish variables periodically 66 | * @param timer_rate rate to publish at 67 | */ 68 | void publishOnTimer(int timer_rate); 69 | 70 | /** 71 | * @brief ADSNode::Subscriber send the ADS device the received commands 72 | * @param msg the command message 73 | * @return true if the command was successfully sent 74 | */ 75 | bool Subscriber(const ros_ads_msgs::ADS::ConstPtr& msg); 76 | 77 | private: 78 | 79 | bool m_publish{false}; /*!< Control to publish event message */ 80 | bool m_configOK{false}; /*!< Configuration state to the modbus device */ 81 | 82 | int m_rate_update; /*!< update and event detection rate */ 83 | int m_rate_state; /*!< state publication rate */ 84 | int m_rate_publish; /*!< periodical publish rate */ 85 | 86 | map> m_publish_on_timer; /*!< caracteristics of variables to publish periodically */ 87 | map> m_publish_on_event; /*!< caracteristics of variables to publish on event */ 88 | map> m_variables; /*!< caracteristics of declared variables */ 89 | mutex m_publish_on_timer_guard; /*!< m_publish_on_timer map guard */ 90 | mutex m_publish_on_event_guard; /*!< m_publish_on_event map guard */ 91 | mutex m_variables_guard; /*!< m_variables map guard */ 92 | 93 | pair m_checker_temp_value; /*!< temporary value for event detection */ 94 | 95 | RosAds_Interface m_ADS = RosAds_Interface(); /*!< ADS device interface */ 96 | string m_name; /*!< name of the device in the configuration file */ 97 | string m_config_file; /*!< configuration file with the description of the ADS device */ 98 | 99 | ros_ads_msgs::ADS m_on_event_msg; /*!< message to publish on event */ 100 | ros_ads_msgs::ADS m_on_timer_msg; /*!< message to publish on timer */ 101 | ros_ads_msgs::State m_state_msg; /*!< state message */ 102 | 103 | ros::Publisher m_state_publisher; /*!< state publisher */ 104 | ros::Publisher m_on_event_publisher; /*!< on event publisher */ 105 | ros::Publisher m_on_timer_publisher; /*!< periodical publisher */ 106 | ros::Subscriber m_subscriber; /*!< command subscriber */ 107 | 108 | shared_ptr m_update_thread; /*!< update thread */ 109 | shared_ptr m_checker_thread; /*!< event detection thread */ 110 | shared_ptr m_timer_thread; /*!< on timer publisher thread */ 111 | shared_ptr m_state_thread; /*!< state publisher thread */ 112 | }; 113 | 114 | } 115 | 116 | #endif 117 | --------------------------------------------------------------------------------