bool YarpJointDev::getDOF ( yarp::sig::Vector& curDof ) { curDof.clear(); for ( unsigned i = 0; i < _chain->GetNumberOfJoints(); ++i ) curDof.push_back ( 1 ); return true; }
bool skinManager::bottleToVector(const yarp::os::Bottle& b, yarp::sig::Vector& v){ for(int i=0; i<b.size(); i++) if(b.get(i).isDouble() || b.get(i).isInt()) v.push_back(b.get(i).asDouble()); else return false; return true; }
void HOGThread::computeHOG(IplImage* img, yarp::sig::Vector &currHist, double Th, int aperture, bool show) { IplImage * contrast= cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 1); IplImage * arctan= cvCreateImage(cvGetSize(img), IPL_DEPTH_32F, 1); IplImage * mask= cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1); IplImage * edges= cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 1); double max =0; double min =0; IplImage *img_t = cvCreateImage(cvGetSize(img),img->depth,1); IplImage *img_f = cvCreateImage(cvGetSize(img),32,1); IplImage * derivativeX= cvCreateImage(cvGetSize(img),32,1); IplImage * derivativeY= cvCreateImage(cvGetSize(img),32,1); if(img->nChannels>1) cvCvtColor(img,img_t,CV_RGB2GRAY); else cvCopy(img,img_t,0); cvMinMaxLoc(img_t,&min,&max,NULL,NULL,NULL); cvConvertScale(img_t,img_f,1.0/max,0); cvSobel(img_f,derivativeX,1,0,aperture); cvSobel(img_f,derivativeY,0,1,aperture); cvZero(arctan); cvZero(contrast); cvCartToPolar(derivativeX, derivativeY, contrast, arctan, 0); cvThreshold(contrast,contrast,Th,1,CV_THRESH_BINARY); cvConvertScale(contrast,mask,255,0); int n_bins[]={nbins}; float range[]={0,2*CV_PI }; float * ranges[] = {range}; CvHistogram* histTemp=cvCreateHist(1, n_bins, CV_HIST_ARRAY, ranges,1); cvCalcHist(&arctan, histTemp, 0, mask); cvNormalizeHist(histTemp,1); for(int i=0; i<nbins; i++) { float bin_value=cvGetReal1D(histTemp->bins,i); currHist.push_back(bin_value); } cvReleaseImage(&img_f); cvReleaseImage(&derivativeX); cvReleaseImage(&derivativeY); cvReleaseImage(&img_t); cvReleaseImage(&contrast); cvReleaseImage(&arctan); cvReleaseImage(&mask); cvReleaseImage(&edges); cvReleaseHist(&histTemp); }
bool JoypadControlClient::getRawTouch(unsigned int touch_id, yarp::sig::Vector& value) { value.clear(); if(m_rpc_only) { Bottle cmd, response; cmd.addVocab(VOCAB_IJOYPADCTRL); cmd.addVocab(VOCAB_GET); cmd.addVocab(VOCAB_TOUCH); cmd.addVocab(VOCAB_VALUE); cmd.addInt32(touch_id); m_rpcPort.write(cmd, response); if(response.get(0).asVocab() == VOCAB_OK && response .get(1).isFloat64() && response.get(2).isFloat64()) { value.push_back(response.get(1).asFloat64()); value.push_back(response.get(2).asFloat64()); return true; } else { return false; } } else { LockGuard l(m_touchPort.mutex); if(touch_id < m_touchPort.storage.size()/2) { value.push_back(m_touchPort.storage[touch_id * 2]); value.push_back(m_touchPort.storage[touch_id * 2 + 1]); return true; } else { return false; } } }
void YarpJointDev::PosEulerSingle_To_PosAxisAngle ( const std::vector< float >& pos, yarp::sig::Vector& xd, yarp::sig::Vector& od ) { assert ( pos.size() == 6 ); xd.clear(); od.clear(); xd.push_back ( pos[0] ); xd.push_back ( pos[1] ); xd.push_back ( pos[2] ); float xa, ya, za, theta; Euler_To_AxisAngle ( pos[3], pos[4], pos[5], xa, ya, za, theta ); od.push_back ( xa ); od.push_back ( ya ); od.push_back ( za ); od.push_back ( theta ); }
int laserHokuyo::readData(const Laser_mode_type laser_mode, const char* text_data, const int text_data_len, int current_line, yarp::sig::Vector& data) { static char data_block [4000]; if (text_data_len==0) { return HOKUYO_STATUS_ERROR_NOTHING_RECEIVED; } long int timestamp = 0 ; // termination complete check if (text_data_len == 1 && text_data[0] == '\n') { //Decode the data int len = strlen(data_block); for (int value_counter =0; value_counter < len; value_counter+=3) { int value = decodeDataValue(data_block+value_counter, 3); if (value<sensor_properties.DMIN && error_codes==1) { value=sensor_properties.DMAX; } //units are m data.push_back(value/1000.0); break; } return HOKUYO_STATUS_ACQUISITION_COMPLETE; } // check in the first line if it is a valid answer to GD command if (current_line == 0) { data_block[0]='\0'; //resets the datablock; if ((laser_mode == MD_MODE && (text_data[0] != 'M' || text_data[1] != 'D')) || (laser_mode == GD_MODE && (text_data[0] != 'G' || text_data[1] != 'D'))) { #if LASER_DEBUG yDebug("Invalid answer to a MD command: %s\n", text_data); #endif return HOKUYO_STATUS_ERROR_INVALID_COMMAND; } else return HOKUYO_STATUS_OK; } // check in the second line if the status of the sensor is ok if (current_line == 1) { if ((laser_mode == MD_MODE && (text_data[0] != '9' || text_data[1] != '9' || text_data[2] != 'b' )) || (laser_mode == GD_MODE && (text_data[0] != '0' || text_data[1] != '0' || text_data[2] != 'P' ))) { #if LASER_DEBUG yDebug("Invalid sensor status: %s\n", text_data); #endif return HOKUYO_STATUS_ERROR_BUSY; } else return HOKUYO_STATUS_OK; } // verify the checksum for all the following lines if (current_line >= 2) { char expected_checksum = text_data[text_data_len - 2]; if (calculateCheckSum(text_data, text_data_len - 2, expected_checksum) < 0) { #if LASER_DEBUG yDebug("Cheksum error, line: %d: %s\n", current_line, text_data); #endif return HOKUYO_STATUS_ERROR_INVALID_CHECKSUM; } } // read the sensor time stamp in the third line if (current_line == 2) { timestamp = decodeDataValue(text_data, 4); } // read the data in the next lines if (current_line >= 3) { strncat (data_block, text_data, text_data_len-2 ); } //increments the lines counter //current_line++; return HOKUYO_STATUS_OK; }
bool JoypadControlClient::getRawStick(unsigned int stick_id, yarp::sig::Vector& value, JoypadCtrl_coordinateMode coordinate_mode) { value.clear(); if(m_rpc_only || coordinate_mode == IJoypadController::JypCtrlcoord_POLAR) { Bottle cmd, response; int dof, coordmode; coordmode = coordinate_mode == IJoypadController::JypCtrlcoord_CARTESIAN ? VOCAB_CARTESIAN : VOCAB_POLAR; cmd.addVocab(VOCAB_IJOYPADCTRL); cmd.addVocab(VOCAB_GET); cmd.addVocab(VOCAB_STICKDOF); cmd.addVocab(VOCAB_VALUE); cmd.addInt32(stick_id); m_rpcPort.write(cmd, response); if(response.get(0).asVocab() == VOCAB_OK && response.get(1).isInt32()) { dof = response.get(1).asInt32(); } else { return false; } cmd.addVocab(VOCAB_IJOYPADCTRL); cmd.addVocab(VOCAB_GET); cmd.addVocab(VOCAB_STICK); cmd.addVocab(VOCAB_VALUE); cmd.addVocab(coordmode); cmd.addInt32(stick_id); m_rpcPort.write(cmd, response); if(response.get(0).asVocab() == VOCAB_OK) { for(int i = 0; i < dof; i++) { if(response.get(i).isFloat64()) { value.push_back(response.get(i).asFloat64()); } else { return false; } } return true; } else { return false; } } else { LockGuard l(m_stickPort.mutex); int offset = 0; unsigned int i; if(getStickCount(i), stick_id >= i) { yError() << "JoypadControlCLient: GetStick() error.. Stick_id out of bound"; return false; } for(size_t j = 0; j < stick_id; j++) { offset += m_stickDof[j]; } for(size_t i = 0; i < m_stickDof[stick_id]; ++i) { value.push_back(m_stickPort.storage[offset + i]); } return true; } }