diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp index 6a0ce7d7..db427825 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp @@ -68,16 +68,24 @@ void GenericEcSlave::setup_syncs() if (sm_configs_.size() == 0) { syncs_.push_back({0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}); syncs_.push_back({1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}); - syncs_.push_back({2, EC_DIR_OUTPUT, (unsigned int)(rpdos_.size()), rpdos_.data(), EC_WD_ENABLE}); - syncs_.push_back({3, EC_DIR_INPUT, (unsigned int)(tpdos_.size()), tpdos_.data(), EC_WD_DISABLE}); + syncs_.push_back( + {2, EC_DIR_OUTPUT, (unsigned int)(rpdos_.size()), rpdos_.data(), + EC_WD_ENABLE}); + syncs_.push_back( + {3, EC_DIR_INPUT, (unsigned int)(tpdos_.size()), tpdos_.data(), + EC_WD_DISABLE}); } else { for (auto & sm : sm_configs_) { if (sm.pdo_name == "null") { syncs_.push_back({sm.index, sm.type, 0, NULL, sm.watchdog}); } else if (sm.pdo_name == "rpdo") { - syncs_.push_back({sm.index, sm.type, (unsigned int)(rpdos_.size()), rpdos_.data(), sm.watchdog}); + syncs_.push_back( + {sm.index, sm.type, (unsigned int)(rpdos_.size()), + rpdos_.data(), sm.watchdog}); } else if (sm.pdo_name == "tpdo") { - syncs_.push_back({sm.index, sm.type, (unsigned int)(tpdos_.size()), tpdos_.data(), sm.watchdog}); + syncs_.push_back( + {sm.index, sm.type, (unsigned int)(tpdos_.size()), + tpdos_.data(), sm.watchdog}); } } } diff --git a/ethercat_manager/src/ethercat_sdo_srv_server.cpp b/ethercat_manager/src/ethercat_sdo_srv_server.cpp index 24a70132..75b6558d 100644 --- a/ethercat_manager/src/ethercat_sdo_srv_server.cpp +++ b/ethercat_manager/src/ethercat_sdo_srv_server.cpp @@ -24,7 +24,6 @@ #include "ethercat_manager/ec_master_async.hpp" #include "ethercat_manager/data_convertion_tools.hpp" - namespace ethercat_manager { void upload( @@ -60,6 +59,9 @@ void upload( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -70,6 +72,9 @@ void upload( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -78,11 +83,13 @@ void upload( try { buffer2data(data_stream, data_value, data_type, data.target, data.data_size); } catch (SizeException & e) { - delete[] data.target; return_stream << e.what(); response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } return_stream << "SDO upload done successfully"; @@ -91,7 +98,9 @@ void upload( response->sdo_return_value = data_value; response->sdo_return_message = return_stream.str(); - delete[] data.target; + if (nullptr != data.target) { + delete[] data.target; + } RCLCPP_INFO(rclcpp::get_logger("ethercat_sdo_srv_server"), return_stream.str().c_str()); } @@ -122,18 +131,22 @@ void download( data.data_size = data2buffer( data_type, request->sdo_value, data.data, data.data_size); } catch (SizeException & e) { - delete[] data.data; return_stream << e.what(); response->success = false; response->sdo_return_message = return_stream.str(); RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); + if (nullptr != data.target) { + delete[] data.target; + } return; } catch (std::ios::failure & e) { - delete[] data.data; return_stream << "Invalid value for type '" << data_type->name << "'!"; response->success = false; response->sdo_return_message = return_stream.str(); RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -145,6 +158,9 @@ void download( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -155,6 +171,9 @@ void download( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -164,12 +183,13 @@ void download( response->success = true; response->sdo_return_message = return_stream.str(); - delete[] data.data; + if (nullptr != data.target) { + delete[] data.target; + } RCLCPP_INFO(rclcpp::get_logger("ethercat_sdo_srv_server"), return_stream.str().c_str()); } } // namespace ethercat_manager - int main(int argc, char ** argv) { rclcpp::init(argc, argv);