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;
}
}