diff --git a/scenarios/tests/DebugDrawerTest/DebugDrawerTestApp/DebugDrawerTestApp.h b/scenarios/tests/DebugDrawerTest/DebugDrawerTestApp/DebugDrawerTestApp.h index 47686c9267c33184bacc2e70e1ee19c3b66e48cb..12e4ec491be5d23ec49568a1ec8a948120795960 100644 --- a/scenarios/tests/DebugDrawerTest/DebugDrawerTestApp/DebugDrawerTestApp.h +++ b/scenarios/tests/DebugDrawerTest/DebugDrawerTestApp/DebugDrawerTestApp.h @@ -32,7 +32,7 @@ namespace armarx { void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties) { - registry->addObject( Component::create<DebugDrawerTest>(properties) ); + registry->addObject(Component::create<DebugDrawerTest>(properties)); } }; -}; +} diff --git a/source/RobotAPI/components/units/ATINetFTUnit.cpp b/source/RobotAPI/components/units/ATINetFTUnit.cpp index baaf4339ae23c8265142eb3a70302f8e3277a8a6..fe6f98e5e2ef7e4fb2d986b04ee1e784d03dd211 100644 --- a/source/RobotAPI/components/units/ATINetFTUnit.cpp +++ b/source/RobotAPI/components/units/ATINetFTUnit.cpp @@ -47,14 +47,14 @@ void ATINetFTUnit::onConnectComponent() * Read ATINetFT hostname and port from properties * std::string receiverHostName("192.168.1.1:49152"); */ - + std::string ATINetFTReceiverHostName = getProperty<std::string>("ATINetFTReceiveHostName").getValue(); std::string captureName = getProperty<std::string>("CaptureName").getValue(); ARMARX_INFO << "Capture Name " << captureName; size_t pos = captureName.find("0"); - captureNamePrefix = captureName.substr(0,pos); + captureNamePrefix = captureName.substr(0, pos); ARMARX_INFO << "Capture Name Prefix " << captureNamePrefix; - std::string captureNameSuffix = captureName.substr(pos,captureName.size()); + std::string captureNameSuffix = captureName.substr(pos, captureName.size()); ARMARX_INFO << "Capture Name Suffix " << captureNameSuffix; nPaddingZeros = captureNameSuffix.size(); captureNumber = std::stoi(captureNameSuffix); @@ -65,7 +65,7 @@ void ATINetFTUnit::onConnectComponent() recordingStopped = true; recordingCompleted = true; recordingStarted = false; - + /* * Connect to ATINetFT */ @@ -94,7 +94,7 @@ void ATINetFTUnit::onConnectComponent() void ATINetFTUnit::onDisconnectComponent() { ARMARX_IMPORTANT << "Disconnecting from ATINetFT" << flush; - + } @@ -105,27 +105,27 @@ void ATINetFTUnit::onExitComponent() void ATINetFTUnit::periodicExec() { - if (recordingStarted) - - + if (recordingStarted) + + { - - float ftdata[ftDataSize]; - char receivePacketContent[receivePacketSize]; - - if (!receivePacket(receivePacketContent)) + //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways) + std::vector<float> ftdata(ftDataSize); + std::vector<char> receivePacketContent(receivePacketSize); + + if (!receivePacket(receivePacketContent.data())) + { + ARMARX_INFO << "recvfrom() failed :"; + } + else { - ARMARX_INFO << "recvfrom() failed :"; - } - else - { - convertToFTValues(receivePacketContent, ftdata, ftDataSize); - writeFTValuesToFile(ftdata, ftDataSize); - sampleIndex++; - } - + convertToFTValues(receivePacketContent.data(), ftdata.data(), ftDataSize); + writeFTValuesToFile(ftdata.data(), ftDataSize); + sampleIndex++; + } + } - + } PropertyDefinitionsPtr ATINetFTUnit::createPropertyDefinitions() @@ -139,19 +139,20 @@ void ATINetFTUnit::startRecording(const std::string& customName, const Ice::Curr if (remoteTriggerEnabled) { if (recordingCompleted) - { - int bpSize = sendPacketSize; - char bytePacket[bpSize]; - *(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */ - *(uint16_t*)&bytePacket[2] = htons(2); /* per table 9.1 in Net F/T user manual. */ - *(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */ - recordingStarted = sendPacket(bytePacket,bpSize); - + { + int bpSize = sendPacketSize; + //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways) + std::vector<char> bytePacket(bpSize); + *(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */ + *(uint16_t*)&bytePacket[2] = htons(2); /* per table 9.1 in Net F/T user manual. */ + *(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */ + recordingStarted = sendPacket(bytePacket.data(), bpSize); + if (recordingStarted) - { + { recordingStopped = false; recordingCompleted = false; - //TODO build filename captureNamePrefic+customName + //TODO build filename captureNamePrefic+customName recordingFile.open(captureNamePrefix); sampleIndex = 0; } @@ -164,131 +165,138 @@ void ATINetFTUnit::stopRecording(const Ice::Current& c) if (remoteTriggerEnabled) { int bpSize = 8; - char bytePacket[bpSize]; - *(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */ - *(uint16_t*)&bytePacket[2] = htons(0); /* per table 9.1 in Net F/T user manual. */ - *(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */ + //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways) + std::vector<char> bytePacket(bpSize); + *(uint16_t*)&bytePacket[0] = htons(0x1234); /* standard header. */ + *(uint16_t*)&bytePacket[2] = htons(0); /* per table 9.1 in Net F/T user manual. */ + *(uint32_t*)&bytePacket[4] = htonl(0); /* see section 9.1 in Net F/T user manual. */ - recordingStopped = sendPacket(bytePacket,bpSize); + recordingStopped = sendPacket(bytePacket.data(), bpSize); if (recordingStopped) { - recordingFile.close(); - recordingStarted = false; - } - + recordingFile.close(); + recordingStarted = false; + } + } } -bool ATINetFTUnit::sendPacket(char *bytePacket, int bpSize) +bool ATINetFTUnit::sendPacket(char* bytePacket, int bpSize) { if (remoteTriggerEnabled) { - if(sendto(senderSocket, bytePacket, bpSize, 0, (struct sockaddr *)&receiveHostAddr, sizeof(receiveHostAddr))!= bpSize) { - - return false; + if (sendto(senderSocket, bytePacket, bpSize, 0, (struct sockaddr*)&receiveHostAddr, sizeof(receiveHostAddr)) != bpSize) + { + + return false; } return true; } return false; } -bool ATINetFTUnit::receivePacket(char *receiveBuf) +bool ATINetFTUnit::receivePacket(char* receiveBuf) { - - int test = sizeof(receiveHostAddr); - - int receiveint = recvfrom(senderSocket, receiveBuf, receivePacketSize, 0, ( struct sockaddr *) &receiveHostAddr, ((socklen_t*)&test)); - - if (receiveint < 0) + + int test = sizeof(receiveHostAddr); + + int receiveint = recvfrom(senderSocket, receiveBuf, receivePacketSize, 0, (struct sockaddr*) &receiveHostAddr, ((socklen_t*)&test)); + + if (receiveint < 0) { - - return false; + + return false; } - - return true; + + return true; } -void ATINetFTUnit::convertToFTValues(char *receiveBuf, float *ftdata, int ftdataSize) +void ATINetFTUnit::convertToFTValues(char* receiveBuf, float* ftdata, int ftdataSize) { - uint32_t rdt_sequence = ntohl(*(uint32_t*)&receiveBuf[0]); - uint32_t ft_sequence = ntohl(*(uint32_t*)&receiveBuf[4]); - uint32_t status = ntohl(*(uint32_t*)&receiveBuf[8]); - - for( int i = 0; i < ftdataSize; i++ ) { - ftdata[i] = float(int32_t(ntohl(*(uint32_t*)&receiveBuf[12 + i * 4])))/ 1000000.0f; - } + //uint32_t rdt_sequence = ntohl(*(uint32_t*)&receiveBuf[0]); //was an unused variable + //uint32_t ft_sequence = ntohl(*(uint32_t*)&receiveBuf[4]); //was an unused variable + //uint32_t status = ntohl(*(uint32_t*)&receiveBuf[8]); //was an unused variable + + for (int i = 0; i < ftdataSize; i++) + { + ftdata[i] = float(int32_t(ntohl(*(uint32_t*)&receiveBuf[12 + i * 4]))) / 1000000.0f; + } } -void ATINetFTUnit::writeFTValuesToFile(float *ftdata, int ftdataSize) +void ATINetFTUnit::writeFTValuesToFile(float* ftdata, int ftdataSize) { - recordingFile << sampleIndex << " "; - for (int i = 0; i < ftdataSize; i++) - recordingFile << ftdata[i] << " "; - recordingFile << "\n"; + recordingFile << sampleIndex << " "; + for (int i = 0; i < ftdataSize; i++) + { + recordingFile << ftdata[i] << " "; + } + recordingFile << "\n"; } void ATINetFTUnit::establishATINetFTReceiveHostConnection(std::string receiverHostName) { - remoteTriggerEnabled = false; + remoteTriggerEnabled = false; //Construct the server sockaddr_ structure memset(&senderHostAddr, 0, sizeof(senderHostAddr)); - senderHostAddr.sin_family=AF_INET; - senderHostAddr.sin_addr.s_addr=htonl(INADDR_ANY); - senderHostAddr.sin_port=htons(0); + senderHostAddr.sin_family = AF_INET; + senderHostAddr.sin_addr.s_addr = htonl(INADDR_ANY); + senderHostAddr.sin_port = htons(0); //Create the socket - if((senderSocket=socket(AF_INET, SOCK_DGRAM, 0))<0) { + if ((senderSocket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) + { return; } - - if(bind(senderSocket,( struct sockaddr *) &senderHostAddr, sizeof(senderHostAddr))<0) { + + if (bind(senderSocket, (struct sockaddr*) &senderHostAddr, sizeof(senderHostAddr)) < 0) + { return; } - - //Construct the server sockaddr_ structure + + //Construct the server sockaddr_ structure memset(&receiveHostAddr, 0, sizeof(receiveHostAddr)); size_t pos = receiverHostName.find(":"); std::string hostname = receiverHostName.substr(0, pos); - std::string hostport = receiverHostName.substr(pos+1, receiverHostName.size()); + std::string hostport = receiverHostName.substr(pos + 1, receiverHostName.size()); - struct hostent *he; - struct in_addr **addr_list; + struct hostent* he; + struct in_addr** addr_list; int i; - if ( (he = gethostbyname( hostname.c_str() ) ) == NULL) + if ((he = gethostbyname(hostname.c_str())) == NULL) { - // get the host info - herror("gethostbyname"); - return; + // get the host info + herror("gethostbyname"); + return; } - addr_list = (struct in_addr **) he->h_addr_list; + addr_list = (struct in_addr**) he->h_addr_list; - for(i = 0; addr_list[i] != NULL; i++) + for (i = 0; addr_list[i] != NULL; i++) { char ip[100]; - //Return the first one; - strcpy(ip , inet_ntoa(*addr_list[i]) ); + //Return the first one; + strcpy(ip , inet_ntoa(*addr_list[i])); - inet_pton(AF_INET,ip,&receiveHostAddr.sin_addr.s_addr); + inet_pton(AF_INET, ip, &receiveHostAddr.sin_addr.s_addr); - receiveHostAddr.sin_port=htons(atoi(hostport.c_str())); + receiveHostAddr.sin_port = htons(atoi(hostport.c_str())); remoteTriggerEnabled = true; - ARMARX_INFO << "Connection established to " << hostname << ":"<< hostport; - //if((receiverSocket=socket(AF_INET, SOCK_DGRAM, 0))<0) { - // return; - //} - //if(bind(receiverSocket,( struct sockaddr *) &receiveHostAddr, sizeof(receiveHostAddr))<0) { - // return; - //} + ARMARX_INFO << "Connection established to " << hostname << ":" << hostport; + //if((receiverSocket=socket(AF_INET, SOCK_DGRAM, 0))<0) { + // return; + //} + //if(bind(receiverSocket,( struct sockaddr *) &receiveHostAddr, sizeof(receiveHostAddr))<0) { + // return; + //} return; } return; - + } diff --git a/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.cpp index 4e4c9a1717c29d4f9622e302e6c8bb3405b80fd4..e365c63864dbb67a9d0f3784600a1085da283a9b 100644 --- a/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.cpp +++ b/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.cpp @@ -15,9 +15,9 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see <http://www.gnu.org/licenses/>. * - * @package - * @author - * @date + * @package + * @author + * @date * @copyright http://www.gnu.org/licenses/gpl-2.0.txt * GNU General Public License */ @@ -40,9 +40,7 @@ TactileSensor::~TactileSensor() tac_matrix_info_t TactileSensor::getMatrixInformation() { - unsigned char payload[0]; - - Response response = interface->submitCmd(0x30, payload, sizeof(payload), false); + Response response = interface->submitCmd(0x30, nullptr, 0, false); response.ensureMinLength(12); response.ensureSuccess(); @@ -162,8 +160,7 @@ void TactileSensor::stopPeriodicFrameAcquisition(void) { while (1) { - unsigned char payload[0]; - interface->fireAndForgetCmd(0x22, payload, sizeof(payload), false); + interface->fireAndForgetCmd(0x22, nullptr, 0, false); int waitCount = 10; while (waitCount > 0) @@ -221,8 +218,7 @@ void TactileSensor::setThreshold(short threshold) } unsigned short TactileSensor::getThreshold() { - unsigned char payload[0]; - Response response = interface->submitCmd(0x35, payload, sizeof(payload), false); + Response response = interface->submitCmd(0x35, nullptr, 0, false); response.ensureMinLength(2); response.ensureSuccess(); return response.getShort(2); @@ -246,8 +242,7 @@ unsigned char TactileSensor::getFrontEndGain() * Get the currently set analog front-end gain value. The gain is as an integer value ranging from 0 to * 255, where 0 is the most insensitive (lowest gain) and 255 is the most sensitive (highest gain) setting. */ - unsigned char payload[0]; - Response response = interface->submitCmd(0x37, payload, sizeof(payload), false); + Response response = interface->submitCmd(0x37, nullptr, 0, false); response.ensureMinLength(3); response.ensureSuccess(); unsigned char gain = response.getByte(2); @@ -258,8 +253,7 @@ std::string TactileSensor::getSensorType() /* * Return a string containing the sensor type. */ - unsigned char payload[0]; - Response response = interface->submitCmd(0x38, payload, sizeof(payload), false); + Response response = interface->submitCmd(0x38, nullptr, 0, false); response.ensureMinLength(2); response.ensureSuccess(); std::string type = std::string((char*)response.data.data() + 2, response.len - 2); @@ -267,16 +261,14 @@ std::string TactileSensor::getSensorType() } float TactileSensor::readDeviceTemperature() { - unsigned char payload[0]; - Response response = interface->submitCmd(0x46, payload, sizeof(payload), false); + Response response = interface->submitCmd(0x46, nullptr, 0, false); response.ensureMinLength(2); short value = (short)response.getShort(2); return value * 0.1f; } tac_system_information_t TactileSensor::getSystemInformation() { - unsigned char payload[0]; - Response response = interface->submitCmd(0x50, payload, sizeof(payload), false); + Response response = interface->submitCmd(0x50, nullptr, 0, false); response.ensureMinLength(9); response.ensureSuccess(); tac_system_information_t si; @@ -304,8 +296,7 @@ void TactileSensor::setDeviceTag(string tag) } string TactileSensor::getDeviceTag() { - unsigned char payload[0]; - Response response = interface->submitCmd(0x52, payload, sizeof(payload), false); + Response response = interface->submitCmd(0x52, nullptr, 0, false); response.ensureMinLength(2); response.ensureSuccess(); std::string tag = std::string((char*)response.data.data() + 2, response.len - 2); @@ -314,8 +305,7 @@ string TactileSensor::getDeviceTag() bool TactileSensor::tryGetDeviceTag(string& tag) { - unsigned char payload[0]; - Response response = interface->submitCmd(0x52, payload, sizeof(payload), false); + Response response = interface->submitCmd(0x52, nullptr, 0, false); response.ensureMinLength(2); if (response.status == E_NOT_AVAILABLE) diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp index 4bcb989e705ceb248bfeebc0d64d4584af2cd86a..719e3786375d1347bc953d9a0c53c424ed852c05 100644 --- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp +++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp @@ -28,6 +28,9 @@ #include <ArmarXCore/observers/variant/DataFieldIdentifier.h> #include <ArmarXCore/observers/exceptions/local/InvalidChannelException.h> +//ignore errors about extra ; from qwt +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" #include <qwt_plot.h> #include <qwt_plot_curve.h> #include <qwt_plot_panner.h> @@ -38,6 +41,7 @@ #include <qwt_series_data.h> #include <qwt_thermo.h> #include <qwt_scale_draw.h> +#pragma GCC diagnostic pop //QT #include <QTimer> @@ -699,7 +703,7 @@ namespace armarx // ARMARX_IMPORTANT << id; auto dict = JSONObject::ConvertToBasicVariantMap(json, var); - for (const auto& e : dict) + for (const auto & e : dict) { std::string key = id + "." + e.first; // ARMARX_INFO << key << ": " << *VariantPtr::dynamicCast(e.second); @@ -743,7 +747,7 @@ namespace armarx { logstream << (time - logStartTime).toMilliSecondsDouble() << ","; - for (auto& channel : selectedChannels) + for (auto & channel : selectedChannels) { logstream << dataMaptoAppend.at(channel.toStdString())->Variant::getOutputValueOnly(); if (!selectedChannels.endsWith(channel)) diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h index 06dadfba4cf4fa44f79267852a424ff8449a266d..abfb4320194d1304ba88bca04481c76df37882ec 100644 --- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h +++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.h @@ -23,7 +23,11 @@ #ifndef ARMARXPLOTTER_H #define ARMARXPLOTTER_H +//ignore errors about extra ; from qwt +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" #include "ui_ArmarXPlotter.h" +#pragma GCC diagnostic pop // ArmarX #include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h> diff --git a/source/RobotAPI/libraries/core/LinkedPose.h b/source/RobotAPI/libraries/core/LinkedPose.h index c424c0127affd9c73e9d829c1ed81dbf7a7a5046..410da0aac406b34390ac5f06abc921fbb0c47cd5 100644 --- a/source/RobotAPI/libraries/core/LinkedPose.h +++ b/source/RobotAPI/libraries/core/LinkedPose.h @@ -46,6 +46,11 @@ namespace armarx // variant types const VariantTypeId LinkedPose = Variant::addTypeName("::armarx::LinkedPoseBase"); const VariantTypeId LinkedDirection = Variant::addTypeName("::armarx::LinkedDirectionBase"); + inline void suppressWarningUnusedVariableForLinkedPoseAndDirection() + { + ARMARX_DEBUG_S << VAROUT(LinkedPose); + ARMARX_DEBUG_S << VAROUT(LinkedDirection); + } } diff --git a/source/RobotAPI/libraries/core/math/Trigonometry.h b/source/RobotAPI/libraries/core/math/Trigonometry.h index e3abce2577e76102633b216ea6777bba47716ef5..2bfa53108b6a5a564b06f10bfff9e83dccacdee7 100644 --- a/source/RobotAPI/libraries/core/math/Trigonometry.h +++ b/source/RobotAPI/libraries/core/math/Trigonometry.h @@ -38,7 +38,7 @@ namespace armarx } static double Deg2RadD(const double angle) { - return angle / 180.0d * M_PI; + return angle / 180.0 * M_PI; } static float Rad2DegF(const float rad) { @@ -46,7 +46,7 @@ namespace armarx } static double Rad2DegD(const float rad) { - return rad / M_PI * 180.0d; + return rad / M_PI * 180.0; } static double GetAngleFromVectorXY(const Eigen::Vector3f& vector)