diff --git a/src/UINode/PingThread.cpp b/src/UINode/PingThread.cpp index 157d44b..291b212 100644 --- a/src/UINode/PingThread.cpp +++ b/src/UINode/PingThread.cpp @@ -61,7 +61,7 @@ void PingThread::stopSystem() double parsePingResult(std::string s) { // 20008 bytes from localhost (127.0.0.1): icmp_req=1 ttl=64 time=0.075 ms - int pos = s.find("time="); + size_t pos = s.find("time="); int found = 0; float ms; if(pos != std::string::npos) diff --git a/src/UINode/tum_ardrone_gui.cpp b/src/UINode/tum_ardrone_gui.cpp index 84a97a1..6b9922d 100644 --- a/src/UINode/tum_ardrone_gui.cpp +++ b/src/UINode/tum_ardrone_gui.cpp @@ -401,7 +401,7 @@ ControlCommand tum_ardrone_gui::calcKBControl() { // clear keys that have not been refreshed for 1s, it is set to "not pressed" for(int i=0;i<8;i++) - isPressed[i] = isPressed[i] && ((lastRepeat[i] + 1000) > getMS()); + isPressed[i] = isPressed[i] && ((lastRepeat[i] + 1000) > (unsigned int)getMS()); ControlCommand c; diff --git a/src/autopilot/ControlNode.cpp b/src/autopilot/ControlNode.cpp index 6a6babd..e56f441 100644 --- a/src/autopilot/ControlNode.cpp +++ b/src/autopilot/ControlNode.cpp @@ -167,12 +167,12 @@ void ControlNode::popNextCommand(const tum_ardrone::filter_stateConstPtr statePt float parameters[10]; // replace macros - if((p = command.find("$POSE$")) != std::string::npos) + if((size_t)(p = command.find("$POSE$")) != std::string::npos) { snprintf(buf,100, "%.3f %.3f %.3f %.3f",statePtr->x,statePtr->y,statePtr->z,statePtr->yaw); command.replace(p,6,buf); } - if((p = command.find("$REFERENCE$")) != std::string::npos) + if((size_t)(p = command.find("$REFERENCE$")) != std::string::npos) { snprintf(buf,100, "%.3f %.3f %.3f %.3f",parameter_referenceZero.pos[0],parameter_referenceZero.pos[1],parameter_referenceZero.pos[2],parameter_referenceZero.yaw); command.replace(p,11,buf); diff --git a/src/autopilot/DroneController.cpp b/src/autopilot/DroneController.cpp index e07575e..8153790 100644 --- a/src/autopilot/DroneController.cpp +++ b/src/autopilot/DroneController.cpp @@ -56,7 +56,7 @@ ControlCommand DroneController::update(tum_ardrone::filter_stateConstPtr state) TooN::Vector<3> pose = TooN::makeVector(state->x, state->y, state->z); double yaw = state->yaw; TooN::Vector<4> speeds = TooN::makeVector(state->dx, state->dy, state->dz, state->dyaw); - ptamIsGood = state->ptamState == state->PTAM_BEST || state->ptamState == state->PTAM_GOOD || state->ptamState == state->PTAM_TOOKKF; + ptamIsGood = state->ptamState == (unsigned int)state->PTAM_BEST || state->ptamState == (unsigned int)state->PTAM_GOOD || state->ptamState == (unsigned int)state->PTAM_TOOKKF; scaleAccuracy = state->scaleAccuracy; // calculate (new) errors. @@ -96,7 +96,7 @@ void DroneController::setTarget(DronePosition newTarget) char buf[200]; snprintf(buf,200,"New Target: xyz = %.3f, %.3f, %.3f, yaw=%.3f", target.pos[0],target.pos[1],target.pos[2],target.yaw); - ROS_INFO(buf); + ROS_INFO("%s", buf); if(node != NULL) node->publishCommand(std::string("u l ") + buf); diff --git a/src/autopilot/KI/KIAutoInit.cpp b/src/autopilot/KI/KIAutoInit.cpp index db22715..fa100e7 100644 --- a/src/autopilot/KI/KIAutoInit.cpp +++ b/src/autopilot/KI/KIAutoInit.cpp @@ -135,7 +135,7 @@ bool KIAutoInit::update(const tum_ardrone::filter_stateConstPtr statePtr) } else // time is up, take second KF { - if(statePtr->ptamState == statePtr->PTAM_INITIALIZING) // TODO: ptam status enum, this should be PTAM_INITIALIZING + if(statePtr->ptamState == (unsigned int)statePtr->PTAM_INITIALIZING) // TODO: ptam status enum, this should be PTAM_INITIALIZING { node->publishCommand("p space"); stageStarted = getMS(); @@ -155,7 +155,7 @@ bool KIAutoInit::update(const tum_ardrone::filter_stateConstPtr statePtr) case WAIT_FOR_SECOND: // am i done? - if(statePtr->ptamState == statePtr->PTAM_BEST || statePtr->ptamState == statePtr->PTAM_GOOD || statePtr->ptamState == statePtr->PTAM_TOOKKF) // TODO: PTAM_GOOD or PTAM_BEST or PTAM_TOOKKF + if(statePtr->ptamState == (unsigned int)statePtr->PTAM_BEST || statePtr->ptamState == (unsigned int)statePtr->PTAM_GOOD || statePtr->ptamState == (unsigned int)statePtr->PTAM_TOOKKF) // TODO: PTAM_GOOD or PTAM_BEST or PTAM_TOOKKF { controller->setTarget(DronePosition( TooN::makeVector(statePtr->x,statePtr->y,statePtr->z),statePtr->yaw)); diff --git a/src/stateestimation/EstimationNode.cpp b/src/stateestimation/EstimationNode.cpp index fcce315..85e6824 100644 --- a/src/stateestimation/EstimationNode.cpp +++ b/src/stateestimation/EstimationNode.cpp @@ -531,7 +531,7 @@ void EstimationNode::reSendInfo() // parse PTAM message std::string ptamMsg = ptamWrapper->lastPTAMMessage; int kf, kp, kps[4], kpf[4]; - int pos = ptamMsg.find("Found: "); + size_t pos = ptamMsg.find("Found: "); int found = 0; if(pos != std::string::npos) found = sscanf(ptamMsg.substr(pos).c_str(),"Found: %d/%d %d/%d %d/%d %d/%d Map: %dP, %dKF", @@ -543,8 +543,6 @@ void EstimationNode::reSendInfo() else snprintf(bufp,200,"Map: -"); - lastNavdataReceived.batteryPercent; - std::string status = ""; switch( lastNavdataReceived.state) { diff --git a/src/stateestimation/PTAM/MiniPatch.cc b/src/stateestimation/PTAM/MiniPatch.cc index 05c999a..db44002 100644 --- a/src/stateestimation/PTAM/MiniPatch.cc +++ b/src/stateestimation/PTAM/MiniPatch.cc @@ -37,7 +37,6 @@ bool MiniPatch::FindPatch(CVD::ImageRef &irPos, vector &vCorners, std::vector *pvRowLUT) { - ImageRef irCenter = irPos; ImageRef irBest; int nBestSSD = mnMaxSSD + 1; ImageRef irBBoxTL = irPos - ImageRef(nRange, nRange); diff --git a/src/stateestimation/PTAMWrapper.cpp b/src/stateestimation/PTAMWrapper.cpp index 30fb118..9241dba 100644 --- a/src/stateestimation/PTAMWrapper.cpp +++ b/src/stateestimation/PTAMWrapper.cpp @@ -196,7 +196,7 @@ void PTAMWrapper::run() snprintf(charBuf,200,"Video resolution: %d x %d",frameWidth,frameHeight); - ROS_INFO(charBuf); + ROS_INFO("%s", charBuf); node->publishCommand(std::string("u l ")+charBuf); // create window @@ -498,13 +498,13 @@ void PTAMWrapper::HandleFrame() //filter->useScalingFixpoint = true; snprintf(charBuf,500,"locking scale fixpoint to %.3f %.3f %.3f",PTAMResultTransformed[0], PTAMResultTransformed[1], PTAMResultTransformed[2]); - ROS_INFO(charBuf); + ROS_INFO("%s", charBuf); node->publishCommand(std::string("u l ")+charBuf); } // ----------------------------- Take KF? ----------------------------------- - if(!mapLocked && isVeryGood && (forceKF || mpMap->vpKeyFrames.size() < maxKF || maxKF <= 1)) + if(!mapLocked && isVeryGood && (forceKF || (int)(mpMap->vpKeyFrames.size()) < maxKF || maxKF <= 1)) { mpTracker->TakeKF(forceKF); forceKF = false; @@ -572,7 +572,7 @@ void PTAMWrapper::HandleFrame() fle->flush(); fle->close(); - printf("FLUSHED %d KEYPOINTS to file pointcloud.txt\n\n",mapPointsTransformed.size()); + printf("FLUSHED %lu KEYPOINTS to file pointcloud.txt\n\n",mapPointsTransformed.size()); flushMapKeypoints = false; } @@ -812,12 +812,12 @@ TooN::Vector<3> PTAMWrapper::evalNavQue(unsigned int from, unsigned int to, bool ) { int frontStamp = getMS(cur->header.stamp); - if(frontStamp < from) // packages before: delete + if(frontStamp < (int)from) // packages before: delete { //navInfoQueue.pop_front(); skipped++; } - else if(frontStamp >= from && frontStamp <= to) + else if(frontStamp >= (int)from && frontStamp <= (int)to) { if(firstAdded == 0) { @@ -922,7 +922,7 @@ void PTAMWrapper::newImage(sensor_msgs::ImageConstPtr img) // copy to mimFrame. // TODO: make this threadsafe (save pointer only and copy in HandleFrame) - if(mimFrameBW.size().x != img->width || mimFrameBW.size().y != img->height) + if(mimFrameBW.size().x != (int)(img->width) || mimFrameBW.size().y != (int)(img->height)) mimFrameBW.resize(CVD::ImageRef(img->width, img->height)); memcpy(mimFrameBW.data(),cv_ptr->image.data,img->width * img->height);