/**
* Read data from the robot_info parameter and construct the offset coordinates
* of the sensors
*/
void SegmentStitching::populateSensorPositions(ros::NodeHandle handle){
ROSUtil::getParam(handle, "/robot_info/ir_error_threshold", sensorUpperLimit);
int numSensors;
ROSUtil::getParam(handle, "/robot_info/num_sensors", numSensors);
std::string xSuffix;
ROSUtil::getParam(handle, "/robot_info/sensor_x_suffix", xSuffix);
std::string ySuffix;
ROSUtil::getParam(handle, "/robot_info/sensor_y_suffix", ySuffix);
std::string zSuffix;
ROSUtil::getParam(handle, "/robot_info/sensor_z_suffix", zSuffix);
std::vector<std::string> sensor_names;
ROSUtil::getParam(handle, "/robot_info/sensor_names", sensor_names);
for (size_t i = 0; i < sensor_names.size(); i++) {
std::string xOffParam = std::string("/robot_info/" + sensor_names[i] + xSuffix);
std::string yOffParam = std::string("/robot_info/" + sensor_names[i] + ySuffix);
std::string zOffParam = std::string("/robot_info/" + sensor_names[i] + zSuffix);
std::string rotParam = std::string("/robot_info/" + sensor_names[i] + "_rotation");
float xoff;
ROSUtil::getParam(handle, xOffParam, xoff);
float yoff;
ROSUtil::getParam(handle, yOffParam, yoff);
float zoff;
ROSUtil::getParam(handle, zOffParam, zoff);
float rot;
ROSUtil::getParam(handle, rotParam, rot);
sensors.push_back(IRSensor(sensor_names[i], xoff, yoff, zoff, rot));
ROS_INFO_STREAM(sensors[i]);
std::cout << sensor_names[i] << " pcl point: " << sensors[i].asPCLPoint() << std::endl;
}
}
/**
* @todo test
*/
void getDistance(void){
if (eventDue(&echoCanFire)){
fireEcho();
}
if (IRFlags.idle){
getNewIRDistance();
}
testUSState();
IRSensor();
if(USFlags.distanceReady && IRFlags.distanceReady){
trackFlags.IRAccuracy = 1; ///@todo change these to bits
trackFlags.USAccuracy = 1;
if (IRDistance < 300 || IRDistance > 800){
trackFlags.IRAccuracy = 0;
}
if (USValues.distance < 400){
trackFlags.USAccuracy = 0;
}
distance = trackFlags.USAccuracy*USValues.distance + trackFlags.IRAccuracy*IRDistance;
if (trackFlags.USAccuracy == trackFlags.IRAccuracy){
distance = distance>>1;
}
/**
* @brief follows the target and stores the distance
*
*/
void follow(void){
static unsigned char followState = 0;
static unsigned char subFollowState = 0;
static int lastIndex = 0;
static int depth;
static int dir;
static int point = 0;
/// Setup
if( followState == CHECK_STATIONARY ){
/// Run a measurement if the IR is not busy
if (IRFlags.idle){
getNewIRDistance();
}
IRSensor();
if ( IRFlags.distanceReady ){ ///< IR sensor has a new distance to process
if ( IRDistance != 0 ) { ///< If we see the ball, stay where we are
getDistance();
return;
} else {
lastKnownAzimuth = currentAzimuth; ///< Store the current position
lastKnownElevation = currentElevation; ///< Store the current elevation
followState = CHECK_NEIGHBOURS; ///< If we missed the ball, check our surroundings
depth = 1; ///< Start searching nearest neighbours
point = 0; ///< Restart point
}
} else {
getDistance();
return; ///< If we haven't got a distance yet, we exit
}
} else if ( followState == CHECK_NEIGHBOURS ){
if ( depth < 3 ){
if ( point < 8 ){
if( subFollowState == CALCULATE_NEXT_POSITION ){ ///< Calculate a new position
dir = ( lastIndex + point ) & 0x07; ///< Calculate which direction to move to
updateCCPServoAngle( depth*trackPositions[dir][AZIM] + lastKnownAzimuth, depth*trackPositions[dir][ELEV] + lastKnownElevation); ///< Move the servo to the next position
setTimeTag( 30, &servoFinished ); ///< Give the servo 10ms to move
subFollowState = IR_MEASUREMENT; ///< Move to purgatory
} else if( subFollowState == IR_MEASUREMENT ){
if( eventDue( &servoFinished )){ ///< Give the servo 10ms to travel
if( IRFlags.idle ){ ///< The IR sensor is not busy
getNewIRDistance();
}
IRSensor();
if ( IRFlags.distanceReady ){ ///< A new measurement is available
if( IRDistance ){ ///< Check if the IRSensor saw something
lastIndex = dir; ///< Store dir as the new lastIndex
followState = CHECK_STATIONARY; ///< Move back to CHECK_STATIONARY
lastKnownAzimuth = currentAzimuth; ///< We have found the ball, so store its position
lastKnownElevation = currentElevation;
} else { ///< The ball is not here
point++; ///< We check the next point
subFollowState = CALCULATE_NEXT_POSITION; ///< We calculate the next servo place
}
}
}
}
} else {
point = 0; ///< We start itterating again
depth++; ///< We move out further
}
} else { ///< The ball is completely lost
clearUSSamplePerEstimate(); ///< Move to single sample to find
trackFlags.targetFound = 0; ///< We lost the ball
depth = 1; ///< Reset depth for next search
}
}
/// Update the distance at the prespecified rate
//getDistance();
// if (eventDue(&echoCanFire)){
// fireEcho();
// }
// testUSState();
// if ( USFlags.distanceReady ){
// distance = USValues.distance;
// USFlags.distanceReady = 0;
// trackFlags.distanceReady = 1;
// }
}
