Skip to content
Snippets Groups Projects
Commit 4c64b516 authored by Raphael Grimm's avatar Raphael Grimm
Browse files

some changes to build the package with -Wpedantic and -Werror on gcc4.8

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